LEGOで作るBluetooth遠隔操作ラジコン（Arduinoコマンド、ESP32系マイコンATOM使用）

#include <M5StickCPlus.h>     // ヘッダーファイル
#include "BluetoothSerial.h"
BluetoothSerial SerialBT;

// 変数宣言
String slave_name = "RC_CAR";       // スレーブ側の接続名
String master_name = "CONTROLLER";  // マスターの接続名
bool connected = 0;     // 接続状態格納用
int connect_count = 3;  // 再接続実行回数
String data = "";       // 受信データ格納用
int btn_pw = 0;         // 電源ボタン状態取得用
float battery;          //バッテリー残量表示用
float x_data;           // ジョイスティックX軸（角度サーボ）
float y_data;           // ジョイスティックY軸（回転サーボ）
int press;              // ジョイスティック押し込みボタン
int btn_a = 0;          // 本体ボタンA動作確認用
/********************** 再起動（リスタート）処理 **********************/
void restart() {
  // 電源ボタン状態取得（1秒以下のONで「2」1秒以上で「1」すぐに「0」に戻る）
  btn_pw = M5.Axp.GetBtnPress();
  if (btn_pw == 2) {  // 電源ボタン短押し（1秒以下）なら
    ESP.restart();    // 再起動
  }
}
// 初期設定 ----------------------------------------------------------
void setup(){
  M5.begin();         // 本体初期化
  Serial.begin(9600); // シリアル出力初期化
  Wire.begin(32, 33); // Grove端子をI2C設定（SDA, SCL）

  // LCD表示設定
  M5.Axp.ScreenBreath(9); // 液晶画面明るさレベル（標準12）
  M5.Lcd.setTextFont(4);  // フォント設定

  // Bluetooth接続開始
  SerialBT.begin(master_name, true);  // マスターとして初期化。trueを書くとマスター、省略かfalseを指定でスレーブ
  M5.Lcd.print("Bluetooth\nSearch!\nRC_CAR\n.");
  Serial.print("Bluetooth\nSearch!\nRC_CAR\n.");

  // Bluetooth接続実行
  while (connected == 0) {    // connectedが0(未接続)なら接続実行を繰り返す
    if (connect_count != 0) { // 再接続実行回数カウントが0でなければ
      connected = SerialBT.connect(slave_name); // 接続実行（接続成功で1を返す）
      M5.Lcd.print(".");
      connect_count--;        // 再接続実行回数カウント -1
    } else {                  // 再接続実行回数カウントが0なら接続失敗
      M5.Lcd.setTextColor(RED, BLACK);
      M5.Lcd.print("\nFailed!");
      Serial.print("\nFailed!");
      while (1) {restart();}  // 無限ループ(再起動待機)
    }
  }
  // 接続確認
  M5.Lcd.setTextColor(WHITE, BLACK);
  if (connected) {                    // 接続成功なら
    M5.Lcd.setTextColor(CYAN, BLACK);
    M5.Lcd.println("\nConnected!");   // 「Connected!」表示
    Serial.println("\nConnected!");
  } else {                            // 接続失敗なら
    M5.Lcd.setTextColor(RED, BLACK);
    M5.Lcd.println("\nFailed!!");     // 「Failed!!」表示
    Serial.println("\nFailed!!");
    while (1) {restart();}            // 無限ループ(再起動待機)
  }
  delay(1000);                        // 接続結果確認画面表示時間
  // LCD表示リセット
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextColor(WHITE, BLACK);
}
// メイン処理 --------------------------------------------------------
void loop() {
  M5.update();  // ボタン状態更新
  // ジョイスティックI2Cデータ取得
  Wire.requestFrom(0x52, 3);  // I2Cデータ取得（アドレス、バイト数）
  while (Wire.available()) {  // 通信データがあれば
    x_data = Wire.read();     // X軸アナログ値取得（0〜255）
    y_data = Wire.read();     // Y軸アナログ値取得（0〜255）
    press = Wire.read();      // 押し込み時ボタン状態取得（true/false）
  }
  // ボタン操作処理（未使用）
  if (M5.BtnA.wasPressed()) { // ボタンAが押されていれば
    btn_a = !btn_a;           // ボタン状態反転
  }
  // Bluetoothデータ送信（「CR」区切り文字）
  if (connected == 1) {       // connectedが1(接続済)ならデータ送信
    SerialBT.printf("%3.0f,%3.0f,%d,%d\r", x_data, y_data, press, btn_a); // X軸アナログ値, Y軸アナログ値, 押し込み時ボタン状態, 本体ボタンA操作状態
  }
  // 液晶表示
  M5.Lcd.setCursor(0, 0);                     // 座標指定
  M5.Lcd.setTextFont(4);                      // フォント設定
  M5.Lcd.println("Controller");               // 項目表示
  M5.Lcd.printf("X : %3.0f      \n", x_data); // X軸アナログ値
  M5.Lcd.printf("Y : %3.0f      \n", y_data); // Y軸アナログ値
  M5.Lcd.printf("Press : %d      \n", press); // 押し込みボタン状態表示
  M5.Lcd.printf("BtnA : %d      \n", btn_a);  // 本体ボタン操作状態表示

  // スレーブ側からの受信データをLCDに表示
  if (SerialBT.available()) {               // Bluetoothデータ受信で
    data = SerialBT.readStringUntil('\r');  // 区切り文字「CR」までデータ取得
    M5.Lcd.print("BT: ");                   // 受信データ液晶表示
    M5.Lcd.println(data + "  ");            // 液晶表示は改行あり
    Serial.print(data + "\r");              // シリアル出力(「CR」付きで先頭復帰し常時表示)
  } else {                                  // Bluetoothデータ受信なしで
    M5.Lcd.setTextColor(ORANGE, BLACK);
    M5.Lcd.println("LOST!             ");   // 警告表示
    M5.Lcd.setTextColor(WHITE, BLACK);
  }
  // バッテリー残量（MAX約4.2V、限界電圧3V）パーセント換算表示
  battery = (M5.Axp.GetBatVoltage() - 3.1) * 100; //バッテリー残量取得、換算
  if (battery > 100) {                            //換算値が100以上なら
    battery = 100;                                //100にする
  }
  M5.Lcd.setTextFont(2);                // フォント設定
  M5.Lcd.setCursor(100, 220);           // 座標
  M5.Lcd.printf("%3.0f%%\n", battery);  //バッテリー残量表示

  restart();  // 再起動（リスタート）処理
  delay(100); // 遅延時間
}

#include <M5Atom.h>
#include "BluetoothSerial.h"
BluetoothSerial SerialBT;
//FastLED（CRGB構造体）設定
CRGB dispColor(uint8_t r, uint8_t g, uint8_t b) {
  return (CRGB)((r << 16) | (g << 8) | b);
}
// 変数宣言
float x_data;           // ジョイスティックX軸（角度サーボ）
float y_data;           // ジョイスティックY軸（回転サーボ）
float speed;            // 回転サーボ速度値換算
int press;              // ジョイスティック押し込みボタン
int btn_a;              // コントローラ側本体ボタンA操作状態
float setting[16][5];   // チャンネル[0〜15]ごとの[周波数, bit数, 0゜パルス時間ms, 最大角度パルス時間ms, 最大動作角度]格納配列
float x_angle;          // X軸角度指示値
float y_round;          // Y軸角度指示値
float duty_angle;       // X軸角度初期値
float duty_y_angle;     // Y軸角度初期値
float read_angle_data;  // X軸角度現在値
float read_round_data;  // Y軸角度現在値
int cnt;                // シリアル出力カウント用
/******************* PWM出力、サーボ仕様設定関数 *******************/
void servo_set(int pin, int channel, float freq, float bit_num, float time_min, float time_max, float angle_max) {
  // サーボ設定値をチャンネルごとの配列に格納
  setting[channel][0] = freq;       // 周波数
  setting[channel][1] = bit_num;    // bit数
  setting[channel][2] = time_min;   // 0゜パルス時間ms
  setting[channel][3] = time_max;   // 最大角度パルス時間ms
  setting[channel][4] = angle_max;  // 最大動作角度
  // PWM設定
  pinMode(pin, OUTPUT);               // PWM用ピンを出力設定
  ledcSetup(channel, freq, bit_num);  // PWM設定（ﾁｬﾝﾈﾙ, 周波数, ビット数）
  ledcAttachPin(pin, channel);        // PWMﾁｬﾝﾈﾙ割り当て（ピン番号, ﾁｬﾝﾈﾙ）
}
/********************** サーボ動作実行関数 **********************/
float servo(int channel, float angle) {
  float resolution;   // 分解能
  float period;       // 周期ms
  float duty_min;     // 0゜Duty計算値
  float duty_max;     // 最大角度Duty計算値
  float duty_active;  // 動作範囲のDuty計算値
  int duty_value;     // PWM出力Duty値（High状態）※整数型として小数点以下切り捨て
  float read_angle;   // PWM出力Duty値からの角度換算

  // サーボ定数算出
  resolution = pow(2, setting[channel][1]);                 // 分解能（2のbit数乗）
  period = 1000 / setting[channel][0];                      // 周期（1000ms / 周波数） 
  duty_min = (resolution / period) * (setting[channel][2]); // 0゜Duty計算値（(分解能/周期)*0゜パルス時間μs）
  duty_max = (resolution / period) * (setting[channel][3]); // 最大角度Duty計算値（(分解能/周期)*最大角度パルス時間μs）
  duty_active = duty_max - duty_min;                        // 動作範囲のDuty計算値
  duty_value = (angle * (duty_active / setting[channel][4])) + duty_min;  // 角度指定値（浮動小数点数） → PWM出力Duty値（整数）換算

  if (duty_value <= duty_min) { // PWM出力Duty値が0゜Duty計算値以下なら
    duty_value = duty_min + 1;  // PWM出力Duty値＋1
  }
  // PWM出力実行
  ledcWrite(channel, duty_value); // 指定したﾁｬﾝﾈﾙにDuty値を指定してPWM出力実行
  read_angle = (ledcRead(channel) - duty_min) * setting[channel][4] / duty_active;  // PWM出力Duty値を取得して角度に換算
  return read_angle;  // 角度を返す
}
// 初期設定 ----------------------------------------------------------
void setup(){
  M5.begin(false, false, true); // 本体初期化（UART, I2C, LED）
  Serial.begin(9600);           // シリアル出力初期化
  // Bluetooth接続開始
  SerialBT.begin("RC_CAR");     // 接続名を指定して初期化。第2引数にtrueを書くとマスター、省略かfalseでスレーブ

  // PWM出力、サーボ仕様設定（サーボごとに使用する端子、ﾁｬﾝﾈﾙ等を設定）
  // PWM出力のﾁｬﾝﾈﾙは 0,1/ 2,3/ 4,5/ 6,7/ 8,9/ 10,11 /12,13 /14,15でペア ※周波数と分解能はペアで同じにする必要があります。
  // (出力端子, ﾁｬﾝﾈﾙ, 周波数, bit数, 0゜パルス時間ms, 最大角度動作時間ms, 最大動作角度)
  servo_set(25, 1, 50, 12, 0.5, 2.4, 180); // ﾁｬﾝﾈﾙ1（ステアリング）：PWM出力、サーボ仕様設定関数呼び出し
  servo_set(21, 2, 50, 12, 0.5, 2.5, 255); // ﾁｬﾝﾈﾙ2（回転駆動）：PWM出力、サーボ仕様設定関数呼び出し
}
// メイン処理 --------------------------------------------------------
void loop() {
  M5.update();  // ボタン状態更新
  // Bluetooth通信データ受信処理
  if (SerialBT.available()) { // コントローラからデータ受信で
    for (int i = 0; i < 25; i++) {              // ドットマトリクスLED25個分繰り返し
      M5.dis.drawpix(i, dispColor(0, 50, 50));  // LED点灯色指定
    }
    // ローカル変数宣言
    int i = 0;    // 取得データカウント用
    String data;  // 受信データ格納用
    data = "";    // データ初期化
    char c;       // 受信データ文字取得用
    // 受信データの文字列をint型に変換して変数に格納
    while (SerialBT.available()) {  // 受信データがある間実行
      c = SerialBT.read();          // Bluetoothデータを文字変数「c」へ格納
      if (c != ',' || c != '\r') {  // 受信データがカンマ「,」でも「CR」でなければ
        data = data + c;            // 文字変数「c」を文字列変数「data」に加算
      }
      if (c == ',') {               // カンマ「,」なら
        switch (i) {                // 「i」で分岐
          case 0:                   // 「0」ならX軸データ
            x_data = data.toInt();  // ジョイスティックX軸データ取得
            break;
          case 1:                   // 「1」ならY軸データ
            y_data = data.toInt();  // ジョイスティックY軸データ取得
            break;
          case 2:                   // 「2」ならスティック押し込みボタン状態
            press = data.toInt();   // ジョイスティック押し込みボタン状態取得（未使用）
            break;
        }
        i++;                        // i +1
        data = "";                  // 文字列変数初期化
      }
      if (c == '\r') {              // 「CR」なら最終データ（コントローラ側本体ボタンA）まで取得完了
        btn_a = data.toInt();       // コントローラ側本体ボタンA操作状態データ取得（未使用）
      } 
    }
    // ジョイスティックX軸受信データをステアリング角度サーボ指示値に換算
    x_angle = 65 * (x_data / 255) + 5;    // X軸ステアリング角度(65°)換算 + 微調整角度

    // ジョイスティックY軸受信データを回転駆動サーボ指示値に換算
    if (y_data >= 115 && y_data <= 139) { // 回転駆動不動領域設定、Y軸がこの範囲なら
      speed = 127;                        // 回転駆動停止（Duty比50%）
    } else if (y_data > 139) {            // ＋動作領域なら
      speed = ((y_data - 127) * 2) + 127; // 中間値（停止状態）からの増加分を2倍
    } else if (y_data < 115) {            // ー動作領域なら
      speed = y_data / 2;                 // 減少分を1/2倍
    }
    if (speed >= 255) {                   // ＋領域の増加分が255以上なら
      speed = 255;                        // 回転サーボ速度値は255（最大値）
    }
    y_round = 255 - speed;                //ジョイスティックY軸 - 回転駆動指示値換算（動作反転指示）

    // ステアリング（角度）サーボ動作実行
    read_angle_data = servo(1, x_angle);  // ﾁｬﾝﾈﾙ1：サーボ動作実行関数呼び出し（ﾁｬﾝﾈﾙ, 角度値）

    // 駆動（360゜回転）サーボ動作実行
    read_round_data = servo(2, y_round);  // ﾁｬﾝﾈﾙ2：サーボ動作実行関数呼び出し（ﾁｬﾝﾈﾙ, 回転指示値）

    // Bluetoothデータ送信
    SerialBT.printf("%3.0f,%3.0f\r", read_angle_data, read_round_data); // 区切り文字「CR」をつけてマスター側へ送信
  } else {
    for (int i = 0; i < 25; i++) {
      M5.dis.drawpix(i, dispColor(100, 30, 0)); // LED点灯
    }
  }
  // シリアル出力
  if (cnt == 10) {  // シリアル出力カウントが10なら
    Serial.printf("X: %3.0f Y: %3.0f P: %d B: %d A: %5.1f R: %5.1f\n", x_data, y_data, press, btn_a, read_angle_data, read_round_data); // X軸アナログ値, Y軸アナログ値, 押し込み時ボタン状態取得, コントローラ側本体ボタンA操作状態, 回転サーボDutyオン幅, 角度サーボDutyオン幅
    cnt = 0;  // シリアル出力カウントリセット
  }
  cnt++;      // シリアル出力カウント加算
  delay(100); // 遅延時間
}