プログラムと電子工作・ラジコンカー（２）プログラムどおりに走行
第1段階で、写真1 のように車体が完成していると思います。
第2段階は、モーターをドライブするスケッチの作成です。スケッチでプログラムした走行パターンどおりに、左折、右折、直進．．．というように走らせるのが第2段階の目標です。
目標
ラジコンカーをスケッチでプログラムした走行パターンどおりに走らせます。例えば、８の字にぐるぐる走行させます。
部品・機材
使用する部品は次のとおりです。
電子部品
M5StickC Plus 1台
第1段階で製作したラジコンカー車体
開発用機材
PC（Windows10 または 11）、開発環境 Arduino-IDE 導入ずみ
USB-A・USB-C ケーブル
開発手順
全体回路図にしたがって M5StickC Plus（電源OFF）をラジコンカー車体（電源OFF）に接続する。
PC と M5StickC Plus を USBケーブルで接続する。
スケッチ robotcar.ino、drive.cpp、drive.h を robotcar フォルダ内に作成する。
Arduino-IDE でスケッチ robotcar.ino を開く。
検証・コンパイルする。
M5StickC Plus に書き込む。
USBケーブルを M5StickC Plus から抜く。
ラジコンカー車体の電源を ONして、スケッチどおりに走行することを確認する。
回路図
スケッチ
PWM制御の準備
PWM制御の準備として、モーターをON/OFFする M5StickC Plus のピンを PWM出力モードに設定します。下記は左車輪のスケッチです。
// 左車輪
#define L_WHEEL_PIN GPIO_NUM_25 /*左車輪ドライブ FETゲート*/
const uint8_t PWM_L_CH = 0; /*PWM出力チャンネル、0または1を指定*/
const uint8_t PWM_RESOLUTION = 16; /*PWM出力の分解能*/
const uint32_t PWM_FREQUENCY = getApbFrequency() / (1U << PWM_RESOLUTION); /*PWM出力周波数*/
ledcSetup(PWM_L_CH, PWM_FREQUENCY, PWM_RESOLUTION);
ledcAttachPin(L_WHEEL_PIN, PWM_L_CH);
ledcWrite(PWM_L_CH, 0); /*ストップ*/M5StickC の PWMのチャンネルは 0～15です。チャンネル0 を左モーターに、チャンネル1 を右モーターに割り当てます。分解能は 1～16ビットです。
PWM出力周波数は、周辺機能の動作周波数を分解能で割り算した値になります。M5StickC の周辺機能の動作周波数は getApbFrequency() 関数で求められ、デフォルトでは 80MHzです。分解能を 16ビットとすると、PWM出力周波数 1220Hzとなります。
PWM制御の準備が終わったら、ledcWrite(PWM_L_CH, 0); で出力を停止しておきます。
PWMによる車輪の回転速度制御
デューティ比で車輪の回転速度を制御します。デューティ比を dutyL とすると、PWM出力は次のようになります。デューティ比は 0.0がストップ、1.0が最高速です。
// 左車輪
float dutyL = 1.0; /*デューティ比 0.0：ストップ、1.0：最高速*/
ledcWrite(PWM_L_CH, uint32_t((1 << PWM_RESOLUTION) * dutyL));液晶ディスプレイに棒グラフを描く
車輪の回転速度に応じた棒グラフを M5StickC Plus の液晶ディスプレイに描きます。これにより、車体の電源スイッチを OFFした状態でも、どのような制御が行われているか棒グラフで確認することができます。入力は dutyL、 dutyR です。
ラジコンカーの走行には関係ありませんが、M5StickC 単体で PWM出力状態を確認できますので、テストには便利です。
// M5StickC Plus は上スイッチが左になる向きに置く
#define LHEIGHT 25 /*1行の高さ（px）*/
// 棒グラフ M5StickC Plus LCD 135x240 pixel
#define X_BARGRAPH 140
#define Y_LMOTOR_BARGRAPH (LHEIGHT*2+LHEIGHT/2)
#define Y_RMOTOR_BARGRAPH (LHEIGHT*3+LHEIGHT/2)
#define WIDTH_BARGRAPH (240-X_BARGRAPH)
#define HIGHT_BARGRAPH 4
//------------------------------------------------------------------------------
// 棒グラフを描く。
// in: float dutyL 左モーターデューティ比、0.0～1.0
// float dutyR 右モーターデューティ比、0.0～1.0
void drawbar(float dutyL, float dutyR)
{
uint32_t left_bar = uint32_t(dutyL * WIDTH_BARGRAPH);
uint32_t right_bar = uint32_t(dutyR * WIDTH_BARGRAPH);
M5.Lcd.fillRect(X_BARGRAPH, Y_LMOTOR_BARGRAPH, left_bar, HIGHT_BARGRAPH, CYAN);
M5.Lcd.fillRect(X_BARGRAPH, Y_RMOTOR_BARGRAPH, right_bar, HIGHT_BARGRAPH, ORANGE);
M5.Lcd.fillRect(X_BARGRAPH + left_bar, Y_LMOTOR_BARGRAPH, WIDTH_BARGRAPH - left_bar, HIGHT_BARGRAPH, BLACK);
M5.Lcd.fillRect(X_BARGRAPH + right_bar, Y_RMOTOR_BARGRAPH, WIDTH_BARGRAPH - right_bar, HIGHT_BARGRAPH, BLACK);
}直進と左右旋回を指示する
走行速度と旋回方向をどのように表現するか迷いました。float forward、float rotate の 2つの変数で表現することにしました。forward は両車輪の回転数を表し、0.0 停止～1.0 最高速です。rotate は左右車輪の回転数差を表し、-1.0～0.0 左旋回、0.0 直進、0.0～+1.0 右旋回です。±1.0のときは、片方の車輪は回転しません。
forward、rotate から左右デューティ比 dutyL、dutyR への変換式は次のようにしました。速度と旋回の制御方法は、もっとよい方法があるかもしれません。
// 前進の係数を計算する。左右共通。
float f_rateL = forward;
float f_rateR = forward;
// 旋回による係数を計算する。
float r_rateL = 1.0 + rotate;
float r_rateR = 1.0 - rotate;
// デューティ比を計算する。
float dutyL = f_rateL * r_rateL;
float dutyR = f_rateR * r_rateR;
// 最大値を超えないように制限する。
// 最小値は制限しない。ただし 0以上。
dutyL = dutyL > COEFF_MAX ? COEFF_MAX : (dutyL < 0.0 ? 0.0 : dutyL);
dutyR = dutyR > COEFF_MAX ? COEFF_MAX : (dutyR < 0.0 ? 0.0 : dutyR);スケッチ全体
長くなりますが、スケッチ全体を掲載します。
loop() の中に、8の字走行のプログラムを書きました。モーターのばらつきによって、同じデューティ比でも回転数が左右同じにならないようです。8の字走行の曲率が異なり、うまく 8の字を描かないかもしれません。wait() の時間を調整するとおおよそ同じ地点に戻ってくるようになります。
drive.cpp
#include "drive.h"
#define LHEIGHT 25 /*1行の高さ（px）M5StickCPlus：25*/
#define L_WHEEL_PIN GPIO_NUM_25 /*左車輪ドライブ FETゲート*/
#define R_WHEEL_PIN GPIO_NUM_26 /*右車輪ドライブ FETゲート*/
// 棒グラフ M5StickC Plus LCD 135x240 pixel
#define X_BARGRAPH 140
#define Y_LMOTOR_BARGRAPH (LHEIGHT*2+LHEIGHT/2)
#define Y_RMOTOR_BARGRAPH (LHEIGHT*3+LHEIGHT/2)
#define WIDTH_BARGRAPH (240-X_BARGRAPH)
#define HIGHT_BARGRAPH 4
// PWM出力のチャンネル
const uint8_t PWM_L_CH = 0;
const uint8_t PWM_R_CH = 1;
// PWM出力の分解能
const uint8_t PWM_RESOLUTION = 16;
// PWM出力の周波数
const uint32_t PWM_FREQUENCY = getApbFrequency() / (1U << PWM_RESOLUTION);
// モーターの最大速度を制限する係数、最大 1.0
const float COEFF_MAX = 1.0;
//------------------------------------------------------------------------------
// モーター駆動を初期化する。
void begin_drive(void)
{
// ピンモードを設定する。
// ※ G25 と G36 は同じピンに接続されているので、
// 使用しない方を無効化する必要がある。
pinMode(L_WHEEL_PIN, OUTPUT); /*PWD出力に使用*/
pinMode(R_WHEEL_PIN, OUTPUT); /*PWD出力に使用*/
// gpio_pulldown_dis(GPIO_NUM_25); /*G25のプルダウンを無効化*/
// gpio_pullup_dis(GPIO_NUM_25); /*G25のプルアップを無効化*/
// PWMの設定
ledcSetup(PWM_L_CH, PWM_FREQUENCY, PWM_RESOLUTION);
ledcSetup(PWM_R_CH, PWM_FREQUENCY, PWM_RESOLUTION);
ledcAttachPin(L_WHEEL_PIN, PWM_L_CH);
ledcAttachPin(R_WHEEL_PIN, PWM_R_CH);
ledcWrite(PWM_L_CH, 0);
ledcWrite(PWM_R_CH, 0);
}
//------------------------------------------------------------------------------
// 指定時間待つ。
// in: uint32_t msec 待ち時間（ミリ秒）
void wait(uint32_t msec)
{
uint32_t t = millis();
while (msec > millis() - t) {
// --- まだ待ち時間が経過していない。
delay(1);
}
}
//------------------------------------------------------------------------------
// モーターを停止する。
void stop(void)
{
ledcWrite(PWM_L_CH, 0);
ledcWrite(PWM_R_CH, 0);
// LCD表示
M5.Lcd.setCursor(0, LHEIGHT*2); /*3行目*/
M5.Lcd.printf("L-M %6d", 0);
M5.Lcd.setCursor(0, LHEIGHT*3); /*4行目*/
M5.Lcd.printf("R-M %6d", 0);
// LCDに棒グラフを描く。
drawbar(0.0, 0.0);
}
//------------------------------------------------------------------------------
// モーター駆動を実行する。
// in: float forward 前進、0.0～1.0、0.0：停止
// float rotate 旋回、-1.0～+1.0、0.0：直進、正：右旋回、負：左旋回
void drive(float forward, float rotate)
{
// 範囲外の処理
if (forward < 0.0) {
forward = 0.0;
}
else if (forward > 1.0) {
forward = 1.0;
}
if (rotate < -1.0) {
rotate = -1.0;
}
else if (rotate > 1.0) {
rotate = 1.0;
}
// 前進の係数を計算する。左右共通。
float f_rateL = forward;
float f_rateR = forward;
// 旋回による係数を計算する。
float r_rateL = 1.0 + rotate;
float r_rateR = 1.0 - rotate;
// デューティ比を計算する。
// ※ ここは自由な式にしてよい。非線形でも．．．
float dutyL = f_rateL * r_rateL;
float dutyR = f_rateR * r_rateR;
// 最大値を超えないように制限する。
// 最小値は制限しない。ただし 0以上。
dutyL = dutyL > COEFF_MAX ? COEFF_MAX : (dutyL < 0.0 ? 0.0 : dutyL);
dutyR = dutyR > COEFF_MAX ? COEFF_MAX : (dutyR < 0.0 ? 0.0 : dutyR);
// PWM駆動する。
ledcWrite(PWM_L_CH, uint32_t((1 << PWM_RESOLUTION) * dutyL));
ledcWrite(PWM_R_CH, uint32_t((1 << PWM_RESOLUTION) * dutyR));
// LCD表示
M5.Lcd.setCursor(0, LHEIGHT*2); /*3行目*/
M5.Lcd.printf("L-M %6d", uint32_t((1 << PWM_RESOLUTION) * dutyL));
M5.Lcd.setCursor(0, LHEIGHT*3); /*4行目*/
M5.Lcd.printf("R-M %6d", uint32_t((1 << PWM_RESOLUTION) * dutyR));
// LCDに棒グラフを描く。
drawbar(dutyL, dutyR);
}
//------------------------------------------------------------------------------
// 棒グラフを描く。
// in: float dutyL 左モーターデューティ比、0.0～1.0
// float dutyR 右モーターデューティ比、0.0～1.0
void drawbar(float dutyL, float dutyR)
{
uint32_t left_bar = uint32_t(dutyL * WIDTH_BARGRAPH);
uint32_t right_bar = uint32_t(dutyR * WIDTH_BARGRAPH);
M5.Lcd.fillRect(X_BARGRAPH, Y_LMOTOR_BARGRAPH, left_bar, HIGHT_BARGRAPH, CYAN);
M5.Lcd.fillRect(X_BARGRAPH, Y_RMOTOR_BARGRAPH, right_bar, HIGHT_BARGRAPH, ORANGE);
M5.Lcd.fillRect(X_BARGRAPH + left_bar, Y_LMOTOR_BARGRAPH, WIDTH_BARGRAPH - left_bar, HIGHT_BARGRAPH, BLACK);
M5.Lcd.fillRect(X_BARGRAPH + right_bar, Y_RMOTOR_BARGRAPH, WIDTH_BARGRAPH - right_bar, HIGHT_BARGRAPH, BLACK);
}drive.h
#ifndef DRIVE_H_INCLUDED
#define DRIVE_H_INCLUDED
#include <M5StickCPlus.h>
void begin_drive(void);
void wait(uint32_t msec);
void stop(void);
void drive(float forward, float rotate);
void drawbar(float dutyL, float dutyR);
#endif /*DRIVE_H_INCLUDED*/robotcar.ino
#include "drive.h"
#define LHEIGHT 25 /*1行の高さ（px）M5StickCPlus：25、M5StickC：15*/
//------------------------------------------------------------------------------
void setup() {
// 電源ON時に 1回だけ実行する処理をここに書く。
M5.begin(); /*M5を初期化する*/
M5.Lcd.setTextSize(2); /*文字サイズは小さめ*/
M5.Lcd.setRotation(3); /*上スイッチが左になる向き*/
Serial.begin(115200); /*デバッグ用のシリアル通信を初期化する*/
// 初期化
begin_drive();
M5.Lcd.fillScreen(BLACK); /*背景を黒にする*/
M5.Lcd.setCursor(0, LHEIGHT*0); /*1行目*/
M5.Lcd.print("robotcar");
delay(2000);
// 最後はモーターを停止する。
stop();
}
//------------------------------------------------------------------------------
void loop() {
// 自動的に繰り返し実行する処理をここに書く。
// drive(前進, 左右);
// 前進：0.0 停止～1.0 最高速
// 左右：0.0～+1.0 右旋回、0.0～-1.0 左旋回
// 8の字走行
// モーターのばらつきにより、うまく 8の字を描かないことがある。
drive(1.0, 0.0); /*直進、最高速*/
wait(250);
drive(0.8, 0.0); /*直進、やや減速*/
wait(250);
drive(0.8, 0.3); /*右旋回*/
wait(3500);
drive(1.0, 0.0); /*直進、最高速*/
wait(250);
drive(0.8, 0.0); /*直進、やや減速*/
wait(250);
drive(0.8, -0.3); /*左旋回*/
wait(3500);
}結果
wait() の時間を調整すると、おおよそ 8の字走行ができました。
停止状態から走り始める瞬間は、大きなパワーが必要なようです。loop() 内の最初は最大速での直進 drive(1.0, 0.0); で始めるのが無難です。一旦走り始めると drive(0.6, 0.0); でもモーターは回り続けるようです。
練習問題
様々な走行パターンを試してください。。
参考
PWM制御
PWM制御の関数については、M5Stack Webサイトにドキュメントがあります。
参考にした関数 ledcSetup()、ledcAttachPin()、ledcWrite() は、次のファイルで定義されています。
C:\Users\＜ユーザ＞\AppData\Local\Arduino15\packages\m5stack\hardware\esp32\2.1.1\cores\esp32\esp32-hal-ledc.h
C:\Users\＜ユーザ＞\AppData\Local\Arduino15\packages\m5stack\hardware\esp32\2.1.1\cores\esp32\esp32-hal-ledc.c
fillrect()
fillrect() 関数については、M5Stack Webサイトにドキュメントがあります。
ライセンス
このページのソースコードは、複製・改変・配布が自由です。営利目的にも使用してかまいませんが、何ら責任を負いません。
謝辞
福武教育文化振興財団から 2023年度助成をいただき製作しました。
この記事が気に入ったらサポートをしてみませんか？