音声でロボットを操作してみる（Node-RED、Gemma2、Faster Whisper、XIAO ESP32C3）

2024年9月4日 2024年9月4日

管理人

はじめに

　今回は音声でロボットの操作を試してみました。今まで試してきたことの、ちょっとしたまとめのような感じです。

　音声でロボットを操作するにあたって、音声を録音し、テキストを抽出して、そのテキストから状況を判定し、操作命令を出すという流れで処理を行っています。すべての処理をNode-REDで呼び出すようにしました。

　Chat GPTはあまり利用してこなかったのですが、今回のプログラム作成にはかなり手伝ってもらいました。何せPythonとJavaScriptとC++を使っているので、大変なことになっています...

　なお、今回は私のよく分からないGPUが載っているWindows 10のノートPCで実行しています。AIツールはすべてローカルで実行しています。

Node-REDでフローを作成する

　最初に全体の処理の流れについて、Node-REDのフローで示します。

▼フローはこちら

[{"id":"90e8f4d49463a350","type":"ollama-chat","z":"3bccaf6e46d3dbf5","name":"Chat","x":250,"y":760,"wires":[["e7ac6c0c24a72f71","bbfaf3102b39c3a4"]]},{"id":"e7ac6c0c24a72f71","type":"debug","z":"3bccaf6e46d3dbf5","name":"debug 63","active":false,"tosidebar":true,"console":false,"tostatus":false,"complete":"payload","targetType":"msg","statusVal":"","statusType":"auto","x":420,"y":760,"wires":[]},{"id":"bbfaf3102b39c3a4","type":"change","z":"3bccaf6e46d3dbf5","name":"","rules":[{"t":"set","p":"payload","pt":"msg","to":"payload.message.content","tot":"msg"}],"action":"","property":"","from":"","to":"","reg":false,"x":420,"y":760,"wires":[["0c528e893c93fbe9","e5c06a2515200630"]]},{"id":"0c528e893c93fbe9","type":"debug","z":"3bccaf6e46d3dbf5","name":"debug 64","active":true,"tosidebar":true,"console":false,"tostatus":false,"complete":"false","statusVal":"","statusType":"auto","x":600,"y":760,"wires":[]},{"id":"426d021a3762f146","type":"venv","z":"3bccaf6e46d3dbf5","venvconfig":"5bb32f2738f10b5a","name":"","code":"import pyaudio\nimport wave\n\ndef record_audio(filename=\"recorded_audio.wav\", duration=3):\n    chunk = 1024  # Number of bytes per chunk\n    sample_format = pyaudio.paInt16  # 16-bit audio\n    channels = 1  # Mono\n    rate = 44100  # Sampling rate\n\n    p = pyaudio.PyAudio()  # Initialize the interface\n\n    # Open the stream and start recording immediately\n    stream = p.open(format=sample_format,\n                    channels=channels,\n                    rate=rate,\n                    frames_per_buffer=chunk,\n                    input=True)\n\n    frames = []  # List to store audio data\n\n    # Record for the specified duration\n    for _ in range(0, int(rate / chunk * duration)):\n        data = stream.read(chunk)\n        frames.append(data)\n\n    # Stop and close the stream\n    stream.stop_stream()\n    stream.close()\n    p.terminate()\n\n    # Save the recorded data to a file\n    with wave.open(filename, 'wb') as wf:\n        wf.setnchannels(channels)\n        wf.setsampwidth(p.get_sample_size(sample_format))\n        wf.setframerate(rate)\n        wf.writeframes(b''.join(frames))\n\n# Immediately start recording and save the audio\nrecord_audio()\n","continuous":false,"x":390,"y":420,"wires":[["9624e4572541a91c"]]},{"id":"7b4c42b178603253","type":"inject","z":"3bccaf6e46d3dbf5","name":"","props":[{"p":"payload"},{"p":"topic","vt":"str"}],"repeat":"","crontab":"","once":false,"onceDelay":0.1,"topic":"","payload":"","payloadType":"date","x":240,"y":420,"wires":[["426d021a3762f146"]]},{"id":"a8c32402f13f0724","type":"pip","z":"3bccaf6e46d3dbf5","venvconfig":"5bb32f2738f10b5a","name":"","arg":"pyaudio","action":"list","tail":false,"x":310,"y":220,"wires":[["bcd377496bd62266","bb753ef73b34bf57"]]},{"id":"1a4a24308399d4f9","type":"inject","z":"3bccaf6e46d3dbf5","name":"","props":[{"p":"payload"},{"p":"topic","vt":"str"}],"repeat":"","crontab":"","once":false,"onceDelay":0.1,"topic":"","payload":"","payloadType":"date","x":160,"y":220,"wires":[["a8c32402f13f0724"]]},{"id":"6db4c6573d2abe81","type":"debug","z":"3bccaf6e46d3dbf5","name":"debug 71","active":true,"tosidebar":true,"console":false,"tostatus":false,"complete":"false","statusVal":"","statusType":"auto","x":480,"y":260,"wires":[]},{"id":"9208f3a0304d90e5","type":"template","z":"3bccaf6e46d3dbf5","name":"","field":"payload","fieldType":"msg","format":"handlebars","syntax":"mustache","template":"{\n    \"model\": \"gemma2:2b-instruct-fp16\",\n    \"messages\": [\n        {\n            \"role\": \"user\",\n            \"content\": \"{{payload}}\"\n        }\n    ]\n}","output":"json","x":600,"y":620,"wires":[["90e8f4d49463a350"]]},{"id":"04f7c3e871bf4209","type":"inject","z":"3bccaf6e46d3dbf5","name":"","props":[{"p":"payload"}],"repeat":"","crontab":"","once":false,"onceDelay":0.1,"topic":"","payload":"前に進め！","payloadType":"str","x":240,"y":580,"wires":[["fe5db020697da1bc"]]},{"id":"fe5db020697da1bc","type":"template","z":"3bccaf6e46d3dbf5","name":"","field":"payload","fieldType":"msg","format":"handlebars","syntax":"mustache","template":"あなたはロボットのオペレーターです。\n音声をテキストに変換して短い命令を送っているので、同音異義語が混じっている可能性があります。\n漢字ではなく、ひらがなで判断してください。\n\n前に進むという文脈の場合はGを返してください。\n後ろに進むという文脈の場合はBを返してください。\n停止するという文脈の場合はSを返してください。\n終了するという文脈の場合もSを返してください。\n右に回転するという文脈の場合はRを返してください。\n左に回転するという文脈の場合はLを返してください。\nそれ以外の文脈ではNを返してください。\n\n停止または終了の意味合いが含まれる場合は、Sだけ返すことを優先してください。\n命令：{{payload}}","output":"str","x":440,"y":620,"wires":[["9208f3a0304d90e5"]]},{"id":"4929b442771d9bd3","type":"mqtt out","z":"3bccaf6e46d3dbf5","name":"","topic":"servo/control","qos":"","retain":"","respTopic":"","contentType":"","userProps":"","correl":"","expiry":"","broker":"f414466379b9f58d","x":450,"y":860,"wires":[]},{"id":"1cc1a4d6f98f8b95","type":"inject","z":"3bccaf6e46d3dbf5","name":"","props":[{"p":"payload"}],"repeat":"","crontab":"","once":false,"onceDelay":0.1,"topic":"","payload":"G","payloadType":"str","x":270,"y":860,"wires":[["4929b442771d9bd3"]]},{"id":"842b15fa75b3cbee","type":"inject","z":"3bccaf6e46d3dbf5","name":"","props":[{"p":"payload"}],"repeat":"","crontab":"","once":false,"onceDelay":0.1,"topic":"","payload":"S","payloadType":"str","x":270,"y":900,"wires":[["4929b442771d9bd3"]]},{"id":"da214776fa4ba1f4","type":"inject","z":"3bccaf6e46d3dbf5","name":"","props":[{"p":"payload"}],"repeat":"","crontab":"","once":false,"onceDelay":0.1,"topic":"","payload":"後退せよ!","payloadType":"str","x":240,"y":620,"wires":[["fe5db020697da1bc"]]},{"id":"eab8b0caade795fe","type":"inject","z":"3bccaf6e46d3dbf5","name":"","props":[{"p":"payload"}],"repeat":"","crontab":"","once":false,"onceDelay":0.1,"topic":"","payload":"停止せよ","payloadType":"str","x":240,"y":660,"wires":[["fe5db020697da1bc"]]},{"id":"9624e4572541a91c","type":"venv","z":"3bccaf6e46d3dbf5","venvconfig":"5bb32f2738f10b5a","name":"","code":"from faster_whisper import WhisperModel\n\nmodel_size = \"tiny\"\n\n# Run on GPU with FP16\n# model = WhisperModel(model_size, device=\"cpu\", compute_type=\"float16\")\n# or run on GPU with INT8\n# model = WhisperModel(model_size, device=\"cuda\", compute_type=\"int8_float16\")\n\n# or run on CPU with INT8\nmodel = WhisperModel(model_size, device=\"cpu\", compute_type=\"int8\")\n\nsegments, info = model.transcribe(\"recorded_audio.wav\", beam_size=5, language=\"ja\")\n\n# print(\"Detected language '%s' with probability %f\" % (info.language, info.language_probability))\n\nfor segment in segments:\n    # print(\"[%.2fs -> %.2fs] %s\" % (segment.start, segment.end, segment.text))\n    path = \"faster-whisper.txt\"\n    with open(path, mode='w', encoding='UTF-8') as f:\n        f.write(segment.text)\n    # print(segment.text)","continuous":false,"x":610,"y":420,"wires":[["8f6e80629720a2e3","cdc828a977a99f29"]]},{"id":"bcd377496bd62266","type":"pip","z":"3bccaf6e46d3dbf5","venvconfig":"5bb32f2738f10b5a","name":"","arg":"faster-whisper","action":"install","tail":false,"x":450,"y":220,"wires":[["6db4c6573d2abe81"]]},{"id":"bb753ef73b34bf57","type":"debug","z":"3bccaf6e46d3dbf5","name":"debug 80","active":true,"tosidebar":true,"console":false,"tostatus":false,"complete":"false","statusVal":"","statusType":"auto","x":300,"y":260,"wires":[]},{"id":"8f6e80629720a2e3","type":"debug","z":"3bccaf6e46d3dbf5","name":"debug 81","active":false,"tosidebar":true,"console":false,"tostatus":false,"complete":"false","statusVal":"","statusType":"auto","x":760,"y":420,"wires":[]},{"id":"cdc828a977a99f29","type":"file in","z":"3bccaf6e46d3dbf5","name":"","filename":"faster-whisper.txt","filenameType":"str","format":"utf8","chunk":false,"sendError":false,"encoding":"none","allProps":false,"x":430,"y":520,"wires":[["fe5db020697da1bc","df872dce2ebc6ac4"]]},{"id":"df872dce2ebc6ac4","type":"debug","z":"3bccaf6e46d3dbf5","name":"debug 82","active":true,"tosidebar":true,"console":false,"tostatus":false,"complete":"false","statusVal":"","statusType":"auto","x":640,"y":520,"wires":[]},{"id":"4058856e36873d5a","type":"inject","z":"3bccaf6e46d3dbf5","name":"","props":[{"p":"payload"},{"p":"topic","vt":"str"}],"repeat":"","crontab":"","once":false,"onceDelay":0.1,"topic":"","payload":"","payloadType":"date","x":240,"y":520,"wires":[["cdc828a977a99f29"]]},{"id":"6c096f85b7750d85","type":"debug","z":"3bccaf6e46d3dbf5","name":"debug 84","active":true,"tosidebar":true,"console":false,"tostatus":false,"complete":"false","statusVal":"","statusType":"auto","x":440,"y":820,"wires":[]},{"id":"e5c06a2515200630","type":"function","z":"3bccaf6e46d3dbf5","name":"function 1","func":"msg.payload = msg.payload.replace(/\\r?\\n/g,\"\")\nmsg.payload = msg.payload.replace(\" \",\"\")\n\nreturn msg;","outputs":1,"timeout":0,"noerr":0,"initialize":"","finalize":"","libs":[],"x":260,"y":820,"wires":[["4929b442771d9bd3","6c096f85b7750d85"]]},{"id":"a0a8aad03900ad58","type":"comment","z":"3bccaf6e46d3dbf5","name":"録音：pyaudio","info":"","x":420,"y":380,"wires":[]},{"id":"9f1eb0d7108010c6","type":"comment","z":"3bccaf6e46d3dbf5","name":"文字起こし：Faster Whisper","info":"","x":680,"y":380,"wires":[]},{"id":"b1b2e9e68627be1b","type":"comment","z":"3bccaf6e46d3dbf5","name":"命令文の作成","info":"","x":230,"y":480,"wires":[]},{"id":"372f8e309ea8f287","type":"comment","z":"3bccaf6e46d3dbf5","name":"動作命令の送信：Ollama","info":"","x":270,"y":720,"wires":[]},{"id":"56b7b27d48211a8c","type":"comment","z":"3bccaf6e46d3dbf5","name":"開始","info":"","x":210,"y":380,"wires":[]},{"id":"5bb32f2738f10b5a","type":"venv-config","venvname":"audio","version":"3.8"},{"id":"f414466379b9f58d","type":"mqtt-broker","name":"","broker":"localhost","port":"1883","clientid":"","autoConnect":true,"usetls":false,"protocolVersion":"4","keepalive":"60","cleansession":true,"autoUnsubscribe":true,"birthTopic":"","birthQos":"0","birthRetain":"false","birthPayload":"","birthMsg":{},"closeTopic":"","closeQos":"0","closeRetain":"false","closePayload":"","closeMsg":{},"willTopic":"","willQos":"0","willRetain":"false","willPayload":"","willMsg":{},"userProps":"","sessionExpiry":""}]

　青色のvenvノードはpython-venvノードのものです。私が開発した、Node-REDでPythonの仮想環境を作成して実行できるノードです。

　今回使っている中でバグが見つかったので、時間が出来たら修正します...

▼python-venvについてはこちら。かなりアップデートしたので、使いやすくなっています。

▼複数の仮想環境を作成して選択できるようになっています。今回はPython 3.8のaudioという仮想環境を作成しました。

▼仮想環境へのパッケージのインストールは、pipノードで行うことができます。

　それぞれの処理について紹介していきます。

Pythonで音声を録音する

　Node-REDでの録音方法を調べていると、Linuxではexecノードでコマンドを実行していたり、UIノードのマイクで録音したりといった具合でした。使い方が間違っているのか、UIノードで録音した音声をそのままファイルに保存して再生しようとすると、スロー音声みたいになっていた記憶があります。

　今回はpython-venvノードでpyaudioを利用したPythonのプログラムで録音することにしました。プログラムの作成はChat GPTにお願いしました。

▼マイクの音声を録音し、ファイルに保存できるようにしました。

　実行してみるとエラーが出ていました。

▼UnicodeDecodeErrorです。

　このエラーはPythonのプログラム自体に問題があるのではなく、python-venvノードに日本語が含まれていると実行できないという問題でした。

▼一文字でも日本語が含まれていると、実行できなくなっていました。

　心当たりはあって、Node-RED上で入力したPythonのプログラムを一度ファイルに保存してから実行しているので、そのときの文字コードで失敗しているのだと思います。この問題には後日対処します...

　さて、日本語を含めないようにして以下のプログラムをChat GPTに出力してもらいました。

▼プログラムはこちら

import pyaudio
import wave

def record_audio(filename="recorded_audio.wav", duration=3):
    chunk = 1024  # Number of bytes per chunk
    sample_format = pyaudio.paInt16  # 16-bit audio
    channels = 1  # Mono
    rate = 44100  # Sampling rate

    p = pyaudio.PyAudio()  # Initialize the interface

    # Open the stream and start recording immediately
    stream = p.open(format=sample_format,
                    channels=channels,
                    rate=rate,
                    frames_per_buffer=chunk,
                    input=True)

    frames = []  # List to store audio data

    # Record for the specified duration
    for _ in range(0, int(rate / chunk * duration)):
        data = stream.read(chunk)
        frames.append(data)

    # Stop and close the stream
    stream.stop_stream()
    stream.close()
    p.terminate()

    # Save the recorded data to a file
    with wave.open(filename, 'wb') as wf:
        wf.setnchannels(channels)
        wf.setsampwidth(p.get_sample_size(sample_format))
        wf.setframerate(rate)
        wf.writeframes(b''.join(frames))

# Immediately start recording and save the audio
record_audio()

▼pyaudioはpipノードでインストールしてください。

▼venvノード内に記述しました。Pythonのインテリセンスが適用されているので、通常のエディタ並みに書きやすくなっています。

　3秒間だけの短い音声が録音され、recorded_audio.wavというファイルに保存されます。このファイルはおそらくNode-REDを起動したときのカレントディレクトリに保存されます。保存先を確実に指定したい場合は、絶対パスに変更してください。

Faster Whisperで文字起こしをする

▼以前の記事で、PythonでWhisperを利用したことがあります。

▼python-venvノードと同じ仕組みで、Node-REDで利用できるノードも作成しました。

　最初はこのWhisperノードを使って処理していたのですが、tinyモデルでも処理に6秒くらいかかっていました。

　もっと高速化したいなと思い、調べているとFaster Whisperというものを見つけました。

▼Faster Whisperのページはこちら。

https://github.com/SYSTRAN/faster-whisper

　これもPythonで実行できて、READMEにサンプルがあったので、そのプログラムを利用することにしました。

▼プログラムはこちら。WhisperModelはお使いの環境に合わせて変更してください。私はGPUではなくCPUで実行しています。

from faster_whisper import WhisperModel

model_size = "tiny"

# Run on GPU with FP16
# model = WhisperModel(model_size, device="cpu", compute_type="float16")
# or run on GPU with INT8
# model = WhisperModel(model_size, device="cuda", compute_type="int8_float16")

# or run on CPU with INT8
model = WhisperModel(model_size, device="cpu", compute_type="int8")

segments, info = model.transcribe("recorded_audio.wav", beam_size=5, language="ja")

for segment in segments:
    path = "faster-whisper.txt"
    with open(path, mode='w', encoding='UTF-8') as f:
        f.write(segment.text)

　Faster Whisperの処理結果は、一旦ファイルに保存しています。これはWhisperをNode-REDで実行したときもそうだったのですが、日本語が含まれていると文字化けしていたためです。

　後で保存したファイルを読み込むようにすると、文字化けを回避できます。

▼Faster Whisperはpipノードでインストールしてください。

▼venvノードに記述しました。

　Faster Whisperのtinyモデルで、2秒ぐらいで録音した音声を処理することができました。とはいえ、速度を重視しているので精度はあまりよくありません。そもそも短い文なので文脈が無いのも影響しているのか、同音異義語になっていることはよくありました。

Gemma2で条件判定を行う

コマンドプロンプトで実行してみる

▼こちらの記事でインストールして実行できるようにしています。

　今回はGemma2の2Bモデルの中でも、2b-instruct-fp16というモデルを使用しています。指令に適したinstructモデルで、精度よりも処理の速さを優先しています。

　Node-REDで実行する前に、コマンドプロンプトで状況を与えてから条件判定を行えるのかを試してみました。

▼gemma2:2b-instruct-fp16をインストールしました。

　あなたはロボットのオペレーターで、前進という文脈だとGを、後進という文脈だとBを返答するように指定しました。

▼前進開始だとG、後退だとBが送信されています。

　さらに条件を追加して、停止するときはSを送信するようにしました。

▼終了するときはSを返答してほしかったのですが、後退終了だとBが返答されました。

▼終了についても条件を追加すると、Sが送られました。

　このように対話しながら判定結果を確認して、条件を追加していくとロボットの操作にも使えそうです。

条件を判定する

▼Chatノードまでの部分の処理です。

　Faster Whisperを実行してファイルに保存したテキストを読み込んで、１つ目のtemplateノードに渡しています。このとき、ロボットを操作するのに必要な条件判定についても含めるようにしました。

▼templateノードの中身はこちら。前進するときはG、停止するときはSを返すなどの指示を出しています。

　mustache記法で、変数は{{}}の中に入れるようになっています。

　この条件判定をどのように設定するのかが工夫のしどころで、想定していない回答が返ってくることがありました。また、Faster Whisperでは同音異義語が返ってくることが多かったので、ひらがなで判定してほしいと伝えています。

　この辺りのAIでの判定はブラックボックスなので、うまく判定されるのかは試してみないと分からないですね。幸い、すぐに実行されるので何回も試してみました。

　２つ目のtemplateノードでollamaノードのChatノードにモデルとメッセージを渡すようにしています。

▼contentは変数にして、１つ目のtemplateノードの文章を代入しています。

　Chatノードに繋いで、debugノードで確認してみました。

▼単語で判定しているような気がします。

▼右、左という単語自体が認識されやすいような気もします。命令が短いので、人間がはっきり発音できる単語を発話するという必要もありそうです。

▼右回転終了だと、Rが送られてからSが送られるということもありました。Sだけ送ってほしいところです。

命令を送信する

▼この部分の処理です。

　msg.payload.message.contentの中にGemma2の返答文があるので、これを取り出します。

▼changeノードの中身はこちら

　changeノードの値だと改行文字と空白が含まれていたので、functionノードで取り除くためのプログラムをJavaScriptで書いています。

▼functionノードの中身はこちら

　最後に、mqtt outノードでservo/controlに対して命令を送信しています。これをロボットで受信します。

MQTT通信でロボットを操作できるようにする

　以前作成した小型の二輪ロボットである、コマゴマ四号を操作対象にしています。マイコンにはXIAO ESP32C3を使っているので、Wi-FiやBluetoothも使えます。

▼コマゴマ四号はイベントで展示していたものです。一機当たり三千円以内で作れます。

▼最近Thingverseに3Dデータを公開しました。

https://www.thingiverse.com/thing:6746264

　Arduino IDEで書き込むためのプログラムを、Chat GPTに書いてもらいました。あとで加速度センサーや超音波センサーの値も使いたかったので、MQTT通信で送信するようになっています。

▼プログラムはこちら。Wi-FiのSSIDやパスワード、MQTTブローカーを起動しているPCのIPアドレスは適宜変更してください。

#include <WiFi.h>
#include <PubSubClient.h>
#include <NewPing.h>
#include <Wire.h>
#include <MPU6050.h>

const char* ssid = "<your SSID>";       // WiFiのSSIDに置き換えてください
const char* password = "<your password>";    // WiFiのパスワードに置き換えてください
const char* mqtt_server = "<your IP>>";  // MQTTブローカーのIPアドレスに置き換えてください

// サーボモータのピン設定
const int ServoPin1 = D1;
const int ServoPin2 = D2;
const int DutyMax = 2300;
const int DutyMin = 700;
int speed1 = 0;
int speed2 = 0;

// 超音波センサーのピン設定
const int triggerPin = D10;
const int echoPin = D9;
NewPing sonar(triggerPin, echoPin, 200);  // 最大距離は200cm

// MPU6050設定
MPU6050 mpu;
int16_t ax_offset = 0, ay_offset = 0, az_offset = 0;
int16_t gx_offset = 0, gy_offset = 0, gz_offset = 0;

WiFiClient espClient;
PubSubClient client(espClient);

// サーボモータの速度制御関数
void ServoSpeed(int pin, int speed) {  // speed: -10 ~ 10
  if (speed != 0) {
    int Duty = map(speed, -10, 10, DutyMin, DutyMax);
    digitalWrite(pin, HIGH);
    delayMicroseconds(Duty);
    digitalWrite(pin, LOW);
    delayMicroseconds(20000 - Duty);  // 20ms周期
  }
}

// MQTT再接続関数
void reconnect() {
  while (!client.connected()) {
    Serial.print("MQTT接続を試みています...");
    if (client.connect("ESP32Client")) {
      Serial.println("接続成功");
      client.subscribe("servo/control");  // サーボ制御のトピックを購読
    } else {
      Serial.print("失敗、rc=");
      Serial.print(client.state());
      Serial.println(" 5秒後に再試行します");
      delay(5000);
    }
  }
}

// MQTTのコールバック関数
void callback(char* topic, byte* payload, unsigned int length) {
  String message;
  for (int i = 0; i < length; i++) {
    message += (char)payload[i];
  }
  Serial.println(message);

  if (message == "G") {
    speed1 = 10;
    speed2 = -10;
  } else if (message == "B") {
    speed1 = -10;
    speed2 = 10;
  } else if (message == "S") {
    speed1 = 0;
    speed2 = 0;
  } else if (message == "R") {
    speed1 = 3;
    speed2 = 3;
  } else if (message == "L") {
    speed1 = -3;
    speed2 = -3;
  }
}

void setup() {
  Serial.begin(115200);

  // サーボモータの初期化
  pinMode(ServoPin1, OUTPUT);
  pinMode(ServoPin2, OUTPUT);
  ServoSpeed(ServoPin1, 0);
  ServoSpeed(ServoPin2, 0);

  // MPU6050の初期化
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050接続に失敗しました");
  }

  // MPU6050のキャリブレーション
  Serial.println("MPU6050をキャリブレーション中...");
  mpu.CalibrateAccel(6);
  mpu.CalibrateGyro(6);
  Serial.println("キャリブレーション完了");

  // WiFi接続
  Serial.print("WiFiに接続中");
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("\nWiFiに接続しました");

  // MQTTサーバー設定と接続
  client.setServer(mqtt_server, 1883);
  client.setCallback(callback);
  reconnect();
}

void loop() {
  // センサーデータをMQTTで送信
  char msg[50];

  ServoSpeed(ServoPin1, speed1);
  ServoSpeed(ServoPin2, speed2);

  if (!client.connected()) {
    reconnect();
  }
  client.loop();

  // 超音波センサーから距離を取得
  float distance = sonar.ping_cm();
  if (distance == 0) distance = 200;  // 最大距離

  // 距離データ送信
  snprintf(msg, sizeof(msg), "%.2f", distance);
  client.publish("sensor/distance", msg);

  // MPU6050から加速度と角速度を取得
  int16_t ax_raw, ay_raw, az_raw;
  int16_t gx_raw, gy_raw, gz_raw;
  mpu.getAcceleration(&ax_raw, &ay_raw, &az_raw);
  mpu.getRotation(&gx_raw, &gy_raw, &gz_raw);

  // 加速度データをgに変換 (±2gレンジの場合)
  float ax = ax_raw / 16384.0;
  float ay = ay_raw / 16384.0;
  float az = az_raw / 16384.0;

  // 角速度データを°/sに変換 (±250°/sレンジの場合)
  float gx = gx_raw / 131.0;
  float gy = gy_raw / 131.0;
  float gz = gz_raw / 131.0;

  // 加速度データ送信
  snprintf(msg, sizeof(msg), "%.2f,%.2f,%.2f", ax, ay, az);
  client.publish("sensor/acceleration", msg);

  // 角速度データ送信
  snprintf(msg, sizeof(msg), "%.2f,%.2f,%.2f", gx, gy, gz);
  client.publish("sensor/gyro", msg);
}

　67~82行目でサーボモーターの速度を変更しています。Node-REDから送られてきたMQTT通信で送られてきた動作命令によって分岐しています。

▼Node-REDのMQTT通信については、こちらの記事をご覧ください。