beavers_hive 2024年01月20日作成 (2024年01月21日更新) © CC BY 4+

製作品

333

beavers_hive 2024年01月20日作成 (2024年01月21日更新) © CC BY 4+

製作品

333

PianoRobo　〜ビーバーの協奏〜

既存のピアノを自動演奏してくれるロボット、そんなものがあったら面白い思いませんか？

この度、Beaver’s Hiveでは、SPRESENSEとM5 Core2を使って、自動演奏するロボットを製作しました！

その名も、 PianoRobo　〜ビーバーの協奏〜 です。

構成としては、M5StackCore2とSpresenseを組み合わせて使っており、Reactを使って作成したアプリから操作できるようになっています！

実際のデモ動画
https://www.youtube.com/watch?v=jQYLRF4vX14

ビーバー単独で使う場合

ビーバーがピアノを弾いてくれます。ビーバーは陸上では機敏に動けないため、早くは弾けませんが、人間のために心を込めて伴奏してくれます。
人と一緒に使う場合

人が右手、ビーバーが左手といった具合で、一緒にピアノを弾くことができます。ピアノ側でスプリット（楽器を鍵盤の半分で変える機能）を使って、ベースやドラムをビーバーに任せることもできます。

PianoRoboの操作はスマホアプリを介して行います。スマホアプリでは以下の２機能が存在します。

Scoreモードはあらかじめ設定した楽譜をinputすることで、ビーバーが曲を弾いてくれるモードです。
曲はスマホから録音することができ、編集モードで後から編集することもできます。

RealTimeモードはスマホの鍵盤を操作するとリアルタイムでビーバーが曲を弾いてくれるモードです。

ピアノにアタッチできる、ロボット筐体をアルミフレームやリニアガイドレール等を用いて構成

側面にラックギアを貼り、エンコーダ付き高トルクDCモータ（+電流センサ）でPI制御を行い、指定位置への迅速で正確かつハイトルクな移動を可能にする。

制御マイコンは高速処理が可能なSPRESENSEと、デバッグに便利なディスプレイ付きのM5StackCore2を使用し、スマホアプリ(webApp)とMQTTで通信を行う。

スマホアプリから鍵盤をタッチしてリアルタイムに演奏したり、楽譜を作成してインプットできる機能を搭載している。

この作品を実装するために必要な部品です。
・SPRESENSE
・M5Core2 * 3
・アルミフレーム
・ラックレール
・平形ギア
・モバイルバッテリ
・12V PDトリガー
・電子キーボード
・サーボモータ*2
・エンコーダ付きDCギアドモータ
・電流センサ
・モータドライバ
・スマホ
・MQTTサーバ
・LEDマトリックスパネル
・LEDテープ
・ビーバー

環境
・ArduinoIDE
・PlatformIO
・React / TypeScript


SPRESENSE-DCモータ制御マイコン
/* Pin No. */
#define SERIAL2_RX 0  //digital
#define SERIAL2_TX 1  //digital
![キャプションを入力できます](https://camo.elchika.com/55e9bdb402d9e7c8e4c72055032f437338805116/687474703a2f2f73746f726167652e676f6f676c65617069732e636f6d2f656c6368696b612f76312f757365722f39643737396532312d386563312d346231632d383733632d3833316466616663663137632f38363066646633362d353730622d343331332d396566372d663964303639643832623038/)
#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;
}

beavers_hive

beavers_hive さんが 2024/01/20 に編集をしました。 (メッセージ: 初版)
beavers_hive さんが 2024/01/20 に編集をしました。
beavers_hive さんが 2024/01/20 に編集をしました。
beavers_hive さんが 2024/01/20 に編集をしました。
beavers_hive さんが 2024/01/21 に編集をしました。

ログインしてコメントを投稿する

PianoRobo　〜ビーバーの協奏〜

この作品は

使い方

機能紹介

Scoreモード

RealTimeモード

システム構成

必要部品

回路図

ソースコード

PianoRobo 〜ビーバーの協奏〜

この作品は

使い方

機能紹介

Scoreモード

RealTimeモード

システム構成

必要部品

回路図

ソースコード

PianoRobo　〜ビーバーの協奏〜