LEGOで作るBluetoothラジコンカー M5StickC Plus2 + ATOM Lite

#include <M5StickCPlus2.h>   // 本体設定用ヘッダーファイル
#include <Wire.h>            // I2C通信用
#include "BluetoothSerial.h" // Bluetooth通信用
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() {
  if (digitalRead(35) == (LOW)) { // 電源が押されていれば
    ESP.restart(); // 再起動
  }
}
// 初期設定 ----------------------------------------------------------
void setup(){
  auto cfg = M5.config(); // 本体初期化
  StickCP2.begin(cfg);

  Serial.begin(9600); // シリアル出力初期化
  Wire.begin(32, 33); // Grove端子をI2C設定（SDA, SCL）

  // LCD表示設定
  M5.Lcd.fillScreen(BLACK); // 背景色
  M5.Lcd.setFont(&fonts::Font4); // フォント

  // 入力端子
  pinMode(35, INPUT_PULLUP); // 入力設定（プルアップ）

  // 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);  // 本体ボタン操作状態表示

  // Bluetoooth受信データを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);
  }

  // バッテリー残量取得（パーセント）
  battery = StickCP2.Power.getBatteryLevel(); // 0〜100％ で取得
  M5.Lcd.setTextFont(2);               // フォント設定
  M5.Lcd.setCursor(90, 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()) { // コントローラからデータ受信で
    M5.dis.drawpix(0, 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) + 10; // X軸ステアリング角度(65°)換算 + 微調整角度

    // ジョイスティックY軸受信データを回転駆動サーボ指示値に換算
    if (y_data >= 110 && y_data <= 129) { // 回転駆動不動領域設定、Y軸がこの範囲なら
      speed = 127;                        // 回転駆動停止（Duty比50%）
    } else {
      speed = y_data; // y軸受信データを回転駆動サーボ指示値へ
    }

    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 {
    M5.dis.drawpix(0, 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(20); // 遅延時間
}