編集履歴一覧に戻る
beavers_hiveのアイコン画像

beavers_hive が 2024年01月20日23時49分35秒 に編集

初版

タイトルの変更

+

PianoRobo 〜ビーバーの協奏〜

タグの変更

+

SPRESENSE

+

M5StackCore2

+

DCモーター

+

ビーバー

メイン画像の変更

メイン画像が設定されました

記事種類の変更

+

製作品

ライセンスの変更

+

(CC BY 4+) Creative Commons Attribution CC BY version 4.0 or later

本文の変更

+

## この作品は 既存のピアノを自動演奏してくれるロボット、そんなものがあったら面白い思いませんか? この度、Beaver’s Hiveでは、SPRESENSEとM5 Core2を使って、自動演奏するロボットを製作しました! その名も、 **PianoRobo 〜ビーバーの協奏〜** です。 構成としては、M5StackCore2とSpresenseを組み合わせて使っており、Reactを使って作成したアプリから操作できるようになっています! 実際のデモ動画 https://www.youtube.com/watch?v=jQYLRF4vX14 ## **使い方** ビーバー単独で使う場合 ビーバーがピアノを弾いてくれます。ビーバーは陸上では機敏に動けないため、早くは弾けませんが、人間のために心を込めて伴奏してくれます。 人と一緒に使う場合 人が右手、ビーバーが左手といった具合で、一緒にピアノを弾くことができます。ピアノ側でスプリット(楽器を鍵盤の半分で変える機能)を使って、ベースやドラムをビーバーに任せることもできます。 ## **機能紹介** PianoRoboの操作はスマホアプリを介して行います。 スマホアプリでは以下の2機能が存在します。 ### Scoreモード Scoreモードはあらかじめ設定した楽譜をinputすることで、ビーバーが曲を弾いてくれるモードです。 曲はスマホから録音することができ、編集モードで後から編集することもできます。 ### RealTimeモード RealTimeモードはスマホの鍵盤を操作するとリアルタイムでビーバーが曲を弾いてくれるモードです。 ## **システム構成** ![キャプションを入力できます](https://camo.elchika.com/1482dcc13e5ac2262e5871174cd9bb341b99c7b3/687474703a2f2f73746f726167652e676f6f676c65617069732e636f6d2f656c6368696b612f76312f757365722f39643737396532312d386563312d346231632d383733632d3833316466616663663137632f35386135306532392d383665642d346332302d393964392d336262656263396266396535/) ピアノにアタッチできる、ロボット筐体をアルミフレームやリニアガイドレール等を用いて構成 側面にラックギアを貼り、エンコーダ付き高トルクDCモータ(+電流センサ)でPI制御を行い、指定位置への迅速で正確かつハイトルクな移動を可能にする。 制御マイコンは高速処理が可能なSPRESENSEと、デバッグに便利なディスプレイ付きのM5StackCore2を使用し、スマホアプリ(webApp)とMQTTで通信を行う。 スマホアプリから鍵盤をタッチしてリアルタイムに演奏したり、楽譜を作成してインプットできる機能を搭載している。 ## 必要部品 この作品を実装するために必要な部品です。 ・SPRESENSE ・M5Core2 * 3 ・アルミフレーム ・ビーバー ・電子キーボード ・サーボモータ*2 ・エンコーダ付きDCギアドモータ ・電流センサ ・モータドライバ ・スマホ ・MQTTサーバ ・LEDマトリックスパネル ・LEDテープ 環境 ・ArduinoIDE ・PlatformIO ・React / TypeScript ## **回路図** ![キャプションを入力できます](https://camo.elchika.com/e95c8f89615db4f096a40455a818d31953ace944/687474703a2f2f73746f726167652e676f6f676c65617069732e636f6d2f656c6368696b612f76312f757365722f39643737396532312d386563312d346231632d383733632d3833316466616663663137632f64626134656132312d346238612d343761642d623435632d396464326363353164343066/) ## **ソースコード** ```:SPRESENSE-DCモータ制御マイコン /* Pin No. */ #define SERIAL2_RX 0 //digital #define SERIAL2_TX 1 //digital #define RMIT_SW_L 2 //digital #define RMIT_SW_R 3 //digital #define PWM1 5 //digital #define PWM2 6 //digital #define TEST1 7 //digital #define TEST2 8 //digital #define TEST3 9 //digital #define TEST4 10 //digital #define TEST5 11 //digital #define INT_A 13 //digital #define INT_B 12 //digital #define VR 4 //analog #define CURR_SENSOR 5 //analog /* Characteristics */ #define VCC5V 4.75 //[V] #define ACS712_05 0.185 //[V/A] #define MOTOR_L 0.002215 // 2.215mH (@10kHz) #define MOTOR_R 1.92 // 1.92ohm (DCR) #define GEAR_RAITO 70.0 // 70:1 #define CPR 64.0 #define AAPC 0.00110 // 0.75(A)*0.5(s)/350(rps) /* LIMIT VALUE */ #define MAX_PWM 255 #define MAX_CURR 1.5 #define MAX_VOLT 11.9 #define MAX_RPS 175 // 70*150rpm/60sec #define MAX_ANGLE 826 // モータ軸回転数 70*1222/33PI #define ANGLE_ERROR 12.50 // /* Control Parameter */ #define PWM_FREQ 20000 #define INT_TIME 500.0 //usec #define SERIAL_TIME 100000 //usec #define SERIAL2_TIME 500000 //usec #define GAIN_C 100.0 #define GAIN_S 10.0 #define RPS_LPF_FC 100.0 //Hz #define GAIN_A 2.50 /* ASCII */ #define ASCII_0 48 #define ASCII_9 57 #define ASCII_LF 10 #define ASCII_CR 13 /* Other */ #define INV_1023 0.0009775 // 1/1023 #define INV_12V 0.08333 // 1/12 #define INV_ACS712_05 5.405 // 1/0.185[V/A] #define INV_AAPC 1 #define CURR_RPF_LOG 5 #define USEC_TO_SEC 0.000001 #define SEC_TO_USEC 1000000.0 #define SERIAL2_DIGIT 5 #define SEND_OK_SIG_TIME 50000 //500ms #define HOME_AJUST_PWM 150 /* Define Variable */ float KP_ACR = GAIN_C * MOTOR_L; float KI_ACR = GAIN_C * MOTOR_R * INT_TIME * USEC_TO_SEC; float KP_ASR = GAIN_S * AAPC; float KI_ASR = GAIN_S * KP_ASR * INT_TIME * USEC_TO_SEC / 10.0; float CONV_RPS = SEC_TO_USEC / (CPR * INT_TIME); float RPS_LPF_CONFF = 1.0 / (2.0 * PI * RPS_LPF_FC * INT_TIME * USEC_TO_SEC + 1.0); float KP_ANGLE_CONTROL = GAIN_A; float KI_ANGLE_CONTROL = GAIN_A / 1000000.0; float CONV_ANGLE = 360.0 / (CPR * GEAR_RAITO); /* Read Value */ signed int ReadVR = 0; signed int OffSetReadCurr = 0; signed int ReadCurr = 0; signed long State = 0; unsigned long RecieveSrial2Val = 10; /* Control Parameter */ unsigned long Time = 0; signed int Pwm = 0; float Curr = 0; float CurrCmd = 0; float VoltCmd = 0; float Rps = 0; float RpsCmd = 0; float Angle = 0; float AngleCmd = 0; signed long StateLog[(int)(SEC_TO_USEC / INT_TIME)]; signed long StateSum = 0; signed long EncoderCnt = 0; /*Flag*/ bool TimerIntFlag = false; bool SendOkSigFlag = false; bool NotSendOkFlag = false; /* Other */ void setup() { pinMode(INT_A, INPUT); pinMode(INT_B, INPUT); pinMode(TEST1, OUTPUT); pinMode(TEST2, OUTPUT); pinMode(TEST3, OUTPUT); pinMode(TEST4, OUTPUT); pinMode(TEST5, OUTPUT); pinMode(RMIT_SW_L, INPUT); pinMode(RMIT_SW_R, INPUT); OffSetReadCurr = analogRead(CURR_SENSOR); Serial.begin(115200); Serial2.begin(115200); Serial.println(""); attachTimerInterrupt(intTimerInt, INT_TIME); attachInterrupt(digitalPinToInterrupt(INT_A), intSigA, CHANGE); attachInterrupt(digitalPinToInterrupt(INT_B), intSigB, CHANGE); } int intTimerInt() { digitalWrite(TEST1, HIGH); Time += INT_TIME; TimerIntFlag = true; EncoderCnt += State; Rps = RPS_LPF_CONFF * Rps + (1.0 - RPS_LPF_CONFF) * (float)State * CONV_RPS; State = 0; digitalWrite(TEST1, LOW); return INT_TIME; } void intSigA() { /* デバッグ用出力 */ /*if (digitalRead(INT_A)) { digitalWrite(TEST3, HIGH); } else { digitalWrite(TEST3, LOW); }*/ //digitalWrite(TEST3, HIGH); if (digitalRead(INT_A) == digitalRead(INT_B)) { State++; } else { State--; } //digitalWrite(TEST3, LOW); } void intSigB() { /* デバッグ用出力 */ /*if (digitalRead(INT_B)) { digitalWrite(TEST4, HIGH); } else { digitalWrite(TEST4, LOW); }*/ //digitalWrite(TEST4, HIGH); //デバッグ用出力 if (digitalRead(INT_A) != digitalRead(INT_B)) { State++; } else { State--; } //digitalWrite(TEST4, LOW); //デバッグ用出力 } void loop() { static bool homeAdjustCompleteFlag = false; /* 原点調整モード */ if (RecieveSrial2Val == 0 || homeAdjustCompleteFlag) { if (digitalRead(RMIT_SW_L)) { EncoderCnt = 0; Serial2.println("0ok"); Serial.println("Home Ajust Complete"); NotSendOkFlag = true; while (digitalRead(RMIT_SW_L)) { motorDrive(HOME_AJUST_PWM); } delay(100); motorDrive(0); homeAdjustCompleteFlag = true; RecieveSrial2Val = 10; delay(500); } else if (!homeAdjustCompleteFlag) { motorDrive(-HOME_AJUST_PWM); Serial.println("Home Ajust Mode"); } else { motorDrive(0); RecieveSrial2Val = 10; homeAdjustCompleteFlag = false; } } /* 緊急停止モード */ else if (digitalRead(RMIT_SW_L) || digitalRead(RMIT_SW_R)) { motorDrive(0); Serial.println("LIMIT SW ON!"); while (!digitalRead(RMIT_SW_L)) { motorDrive(-HOME_AJUST_PWM); Serial.println("Home Ajust Now!"); } while (digitalRead(RMIT_SW_L)) { motorDrive(HOME_AJUST_PWM); } EncoderCnt = 0; delay(100); motorDrive(0); Serial2.println("0ok"); Serial.println("Home Ajust Complete"); RecieveSrial2Val = 10; delay(500); } /* モータ制御モード */ else { homeAdjustCompleteFlag = false; mainControl(); } } void mainControl() { static unsigned int serialCnt = 0; static unsigned int serial2Cnt = 0; static unsigned int sendOkSigCnt = 0; static bool serialFlag = false; static bool serial2Flag = false; static bool okSendFlag = false; if (TimerIntFlag) { digitalWrite(TEST5, HIGH); serialCnt += INT_TIME; serial2Cnt += INT_TIME; if (AngleCmd - ANGLE_ERROR < Angle && Angle < AngleCmd + ANGLE_ERROR) { okSendFlag = true; } if (SendOkSigFlag && okSendFlag) { sendOkSigCnt += INT_TIME; okSendFlag = false; } sensorRead(); AngleCmd = (float)RecieveSrial2Val * 360.0 / GEAR_RAITO; Angle = EncoderCnt * CONV_ANGLE; //RpsCmd = calcAngleControl(AngleCmd, Angle); if (0 < RecieveSrial2Val && RecieveSrial2Val < MAX_ANGLE) { RpsCmd = calcAngleControl(AngleCmd, Angle); } else if (RecieveSrial2Val == 0) { RpsCmd = -20.0; } else { RpsCmd = 0.0; } CurrCmd = calcAsr(RpsCmd, Rps); Curr = (float)(ReadCurr - OffSetReadCurr) * VCC5V * INV_1023 * INV_ACS712_05; VoltCmd = calcAcr(CurrCmd, Curr); Pwm = MAX_PWM * VoltCmd * INV_12V; motorDrive(Pwm); TimerIntFlag = false; digitalWrite(TEST5, LOW); } if (SEND_OK_SIG_TIME <= sendOkSigCnt) { sendOkSigCnt = INT_TIME; SendOkSigFlag = false; if (!NotSendOkFlag) { Serial2.println("ok"); Serial.println("ok"); } else { NotSendOkFlag = false; } } if (serialCnt >= SERIAL_TIME || serialFlag) { serialCnt = INT_TIME; serialFlag = serialOut(); } if (serial2Cnt >= SERIAL2_TIME || serial2Flag) { serial2Cnt = INT_TIME; serial2Flag = serial2Read(); digitalWrite(TEST3, LOW); } } void sensorRead() { ReadVR = analogRead(VR); ReadCurr = analogRead(CURR_SENSOR); } float calcAcr(float iCmd, float iMeas) { float iErr = 0; static float acrSum = 0; static float acrP = 0; static float acrI = 0; iErr = iCmd - iMeas; acrI += KI_ACR * iErr; if (acrI > MAX_VOLT) { acrI = MAX_VOLT; } else if (acrI < -MAX_VOLT) { acrI = -MAX_VOLT; } acrP = KP_ACR * iErr; if (acrP > MAX_VOLT) { acrP = MAX_VOLT; } else if (acrP < -MAX_VOLT) { acrP = -MAX_VOLT; } acrSum = acrI + acrP; if (acrSum > MAX_VOLT) { acrSum = MAX_VOLT; } else if (acrSum < -MAX_VOLT) { acrSum = -MAX_VOLT; } return (acrSum); } float calcAsr(float rpsCmd, float rpsMeas) { float rpsErr = 0; static float asrSum = 0; static float asrP = 0; static float asrI = 0; rpsErr = rpsCmd - rpsMeas; asrI += KI_ASR * rpsErr; if (asrI > MAX_CURR) { asrI = MAX_CURR; } else if (asrI < -MAX_CURR) { asrI = -MAX_CURR; } asrP = KP_ASR * rpsErr; if (asrP > MAX_CURR) { asrP = MAX_CURR; } else if (asrP < -MAX_CURR) { asrP = -MAX_CURR; } asrSum = asrI + asrP; if (asrSum > MAX_CURR) { asrSum = MAX_CURR; } else if (asrSum < -MAX_CURR) { asrSum = -MAX_CURR; } return (asrSum); } float calcAngleControl(float angleCmd, float angleMeas) { float angleErr = 0; static float angleSum = 0; static float angleP = 0; static float angleI = 0; angleErr = angleCmd - angleMeas; angleI += KI_ANGLE_CONTROL * angleErr; if (angleI > MAX_RPS) { angleI = MAX_RPS; } else if (angleI < -MAX_RPS) { angleI = -MAX_RPS; } angleP = KP_ANGLE_CONTROL * angleErr; if (angleP > MAX_RPS) { angleP = MAX_RPS; } else if (angleP < -MAX_RPS) { angleP = -MAX_RPS; } angleSum = angleP + angleI; if (angleSum > MAX_RPS) { angleSum = MAX_RPS; } else if (angleSum < -MAX_RPS) { angleSum = -MAX_RPS; } return (angleSum); } void motorDrive(signed int pwm) { if (pwm == 0) { analogWrite(PWM1, MAX_PWM); analogWrite(PWM2, MAX_PWM); } else if (pwm > 0 && pwm < MAX_PWM) { analogWriteFreq(PWM1, pwm, PWM_FREQ); analogWriteFreq(PWM2, 0, PWM_FREQ); } else if (pwm < 0 && pwm > -MAX_PWM) { analogWriteFreq(PWM1, 0, PWM_FREQ); analogWriteFreq(PWM2, -pwm, PWM_FREQ); } else { analogWrite(PWM1, 0); analogWrite(PWM2, 0); } } bool serial2Read() { static unsigned int cnt = 0; static unsigned int readVal[SERIAL2_DIGIT]; unsigned int readValSum = 0; unsigned int calcVal = 0; signed int receivedData = 0; if (Serial2.available()) { digitalWrite(TEST3, HIGH); receivedData = Serial2.read(); if (ASCII_0 <= receivedData && receivedData <= ASCII_9) { readVal[cnt] = (receivedData - ASCII_0); cnt++; return true; } else if ((receivedData == ASCII_LF || receivedData == ASCII_CR) && cnt > 0) { digitalWrite(TEST4, HIGH); for (int i = 0; i < cnt; i++) { readValSum = 1; for (int j = cnt; j > i + 1; j--) { readValSum *= 10; } calcVal += readValSum * readVal[i]; } cnt = 0; RecieveSrial2Val = calcVal; digitalWrite(TEST4, LOW); SendOkSigFlag = true; return false; } else { return false; } } else { return false; } } bool serialOut() { static unsigned int swCnt = 0; static float serialTime = 0; static signed int serialPwm = 0; static signed long serialEncoderCnt = 0; static unsigned int serialReadVR = 0; static float serialReadCurr = 0; static float serialCurr = 0; static float serialCurrCmd = 0; static float serialVoltCmd = 0; static float serialRps = 0; static float serialRpsCmd = 0; static float serialAngle = 0; static float serialAngleCmd = 0; static float serialRecieveSrial2Val = 0; switch (swCnt) { case 0: digitalWrite(TEST2, HIGH); serialTime = (float)Time * USEC_TO_SEC; serialPwm = Pwm; serialEncoderCnt = EncoderCnt; serialReadVR = ReadVR; serialReadCurr = ReadCurr; serialCurr = Curr; serialCurrCmd = CurrCmd; serialVoltCmd = VoltCmd; serialRps = Rps; serialRpsCmd = RpsCmd; serialAngle = Angle; serialAngleCmd = AngleCmd; serialRecieveSrial2Val = RecieveSrial2Val; break; case 1: Serial.print(serialTime); Serial.print(", "); break; case 2: Serial.print(serialReadVR); Serial.print(", "); break; case 3: Serial.print(serialPwm); Serial.print(", "); break; case 4: Serial.print(serialEncoderCnt); Serial.print(", "); break; case 5: Serial.print(serialReadCurr); Serial.print(", "); break; case 6: Serial.print(serialVoltCmd); Serial.print(", "); break; case 7: Serial.print(serialCurrCmd); Serial.print(", "); break; case 8: Serial.print(serialCurr); Serial.print(", "); break; case 9: Serial.print(serialRpsCmd); Serial.print(", "); break; case 10: Serial.print(serialRps); Serial.print(", "); break; case 11: Serial.print(serialAngle); Serial.print(", "); break; case 12: Serial.print(serialAngleCmd); Serial.print(", "); break; case 13: Serial.print(serialRecieveSrial2Val); Serial.print(", "); break; default: Serial.println(); swCnt = 0; digitalWrite(TEST2, LOW); return false; } swCnt++; return true; } ``` M5側のソースコード ```:M5Core2-集約マイコン #include <WiFi.h> #include <PubSubClient.h> #include <M5Core2.h> #include <ESP32Servo.h> #include <ArduinoJson.h> #define ASCII_LF 10 #define ASCII_CR 13 #define MQTT_MAX_PACKET_SIZE 4096 void move0(); void move(int number); void metoroPlay(); String removeNewlines(const String &str); const char *ssid = "****"; const char *password = "****"; const char *mqttServer = "*****"; const int mqttPort = ****; bool isRealTimeMode = true; bool isLogMode = false; Servo servoHakken; Servo servoKokken; Servo servoShippo; WiFiClient espClient; PubSubClient client(espClient); void serialRead(String &message); bool servoState = false; bool isMotorMove = false; int nowPosition = 0; void beaverSippo(bool isMotorMove) { if (isMotorMove) { servoShippo.write(0); delay(100); servoShippo.write(90); delay(100); } } void servoOn() { isMotorMove = false; servoState = true; M5.Lcd.fillScreen(BLUE); servoHakken.write(90); delay(100); } void servoOff() { client.publish("led", "0"); M5.Lcd.fillScreen(RED); if (servoState == false) { return; } servoHakken.write(0); delay(10); servoState = false; M5.Lcd.fillScreen(BLACK); } void mqttCallbackJson(char *topic, byte *payload, unsigned int length) { M5.Lcd.clear(); M5.Lcd.setCursor(0, 0); M5.Lcd.println("Message arrived"); // ペイロードを文字列に変換 String input = ""; for (int i = 0; i < length; i++) { input += (char)payload[i]; } int numCols = 2; int numRows = input.length() / 8; int array[numRows][numCols]; // M5.Lcd.println(input); int index = 0; for (int row = 0; row < numRows; row++) { for (int col = 0; col < numCols; col++) { String elementString = input.substring(index * 4, index * 4 + 4); int element = elementString.toInt(); array[row][col] = element; M5.Lcd.print(element); index++; } } move0(); metoroPlay(); isMotorMove = false; int nowKey = 0; // 配列の要素を取得して処理 for (int row = 0; row < numRows; row++) { int key = array[row][0]; int length = array[row][1]; M5.Lcd.clearDisplay(); M5.Lcd.print("Key: "); M5.Lcd.println(key); M5.Lcd.print("Length: "); M5.Lcd.println(length); M5.Lcd.clearDisplay(); move(key); if (key != nowKey) { while (true) { if (key == 100) { delay(length); break; } M5.Lcd.clearDisplay(); M5.Lcd.setCursor(0, 0); M5.Lcd.println("waiting..."); if (Serial2.available()) { String message; serialRead(message); if (message.equals("ok")) servoOn(); delay(length); servoOff(); M5.Lcd.println("ok"); break; } // delete message; } } if (key != 100) { nowKey = key; } } } int getMqttNumber(char *topic, byte *payload, unsigned int length) { int extractedNumber = 0; for (int i = 0; i < length; i++) { if (payload[i] >= '0' && payload[i] <= '9') { extractedNumber = extractedNumber * 10 + (payload[i] - '0'); } } return extractedNumber; } int keyNumberToMm(int number) { if (number > 100) { // 黒鍵ロジック 今適当 return (number - 100) * 23; } return number * 23.5; } int getRotationNumber(int distance) { distance = distance + 5; return distance / (35 * 3.1415) * 70; } void move(int number) { client.publish("led", "1"); if (servoState == true) { servoOff(); } M5.Lcd.setCursor(0, 0); if (number == 100) { M5.Lcd.println("off"); return; } else { int distance = keyNumberToMm(number); int diff = distance - nowPosition; if (isMotorMove) { M5.Lcd.println("MotorMoving"); M5.Lcd.printf("%d", getRotationNumber(distance)); return; } M5.Lcd.clearDisplay(); M5.Lcd.println("diff"); M5.Lcd.println(diff); if (diff == 0) { servoOn(); } else { // まずはSpresenseに送信 // okが帰ってきたらサーボがオンになる Serial2.println(getRotationNumber(distance)); nowPosition = distance; M5.Lcd.printf("%d", distance); isMotorMove = true; } } } void mqttCallback(char *topic, byte *payload, unsigned int length) { M5.Lcd.clear(); int number = getMqttNumber(topic, payload, length); move(number); } void reconnect() { while (!client.connected()) { Serial.print("Attempting MQTT connection..."); String clientId = "ESP32Client-"; clientId += String(random(0xffff), HEX); if (client.connect(clientId.c_str())) { Serial.println("connected"); client.subscribe("log2"); } else { Serial.print("failed, rc="); Serial.print(client.state()); Serial.println(" retrying in 5 seconds"); delay(5000); } } } void vibration(int length) { M5.Axp.SetLDOEnable(3, true); delay(length); M5.Axp.SetLDOEnable(3, false); } bool touchJudge(TouchPoint_t atTouchPoint, int x, int y) { int offset = 30; if (atTouchPoint.x > x - offset && atTouchPoint.x < x + offset && atTouchPoint.y > y - offset && atTouchPoint.y < y + offset) { vibration(100); delay(100); return true; } else { return false; } } void initialView() { if (isLogMode) { M5.Lcd.clearDisplay(); M5.Lcd.setCursor(0, 0); M5.Lcd.println("LogMode"); } else { if (isRealTimeMode == false) { M5.Lcd.clearDisplay(); M5.Lcd.setCursor(0, 0); M5.Lcd.drawJpgFile(SD, "/score.jpg"); client.disconnect(); } else { M5.Lcd.clearDisplay(); M5.Lcd.setCursor(0, 0); M5.Lcd.drawJpgFile(SD, "/realtime.jpg"); reconnect(); } } } String removeNewlines(const String &str) { String result = str; result.replace("\r", ""); result.replace("\n", ""); return result; } void setCallBack(bool isRealTimeMode) { client.setCallback(nullptr); if (!isRealTimeMode) { client.setCallback(mqttCallbackJson); } else { client.setCallback(mqttCallback); } } void setup() { M5.begin(); M5.Lcd.println("Connecting to WiFi..."); Serial.begin(115200); Serial2.begin(115200); client.setBufferSize(4096); WiFi.begin(ssid, password); while (WiFi.status() != WL_CONNECTED) { delay(500); Serial.print("."); } servoHakken.setPeriodHertz(50); servoHakken.attach(G19, 500, 2400); servoKokken.attach(G33, 500, 2400); servoShippo.attach(G27, 500, 2400); M5.Lcd.setTextSize(3); M5.Lcd.println("WiFi connected"); M5.Lcd.println("IP address:"); M5.Lcd.println(WiFi.localIP()); Serial.println("connected"); vibration(500); client.setServer(mqttServer, mqttPort); delay(1000); setCallBack(isRealTimeMode); M5.Axp.SetSpkEnable(true); } void loop() { beaverSippo(isMotorMove); if (Serial2.available()) { String message; serialRead(message); if (message.equals("ok")) { servoOn(); } if (message.equals("0ok")) { servoOff(); isMotorMove = false; servoState = false; } // delete message; } if (!client.connected()) { reconnect(); } M5.update(); if (M5.Touch.ispressed()) { TouchPoint_t atTouchPoint = M5.Touch.getPressPoint(); if (touchJudge(atTouchPoint, 200, 200)) { isRealTimeMode = !isRealTimeMode; setCallBack(isRealTimeMode); initialView(); } else if (touchJudge(atTouchPoint, 200, 50)) { // isLogMode = !isLogMode; // initialView(); move0(); } else if (touchJudge(atTouchPoint, 50, 200)) { metoroPlay(); // i2s_write(Speak_I2S_NUMBER, metoro, 35988, &bytes_written, portMAX_DELAY); } } client.loop(); } void metoroPlay() { M5.Spk.DingDong(); delay(300); M5.Spk.DingDong(); delay(300); M5.Spk.DingDong(); delay(300); M5.Spk.DingDong(); } void move0() { servoOff(); Serial2.println("0"); client.publish("led", "2"); while (true) { if (Serial2.available()) { String message; serialRead(message); if (message.equals("0ok")) { break; } // delete message; } } } void serialRead(String &message) { char a = Serial2.read(); int b = 0; char mem[10] = {0}; while (a != ASCII_CR && a != ASCII_LF) { mem[b++] = a; a = Serial2.read(); if (a == -1) { break; } } message = String(mem); b = 0; } ```