(無線電送)M5NanoC6、並列二輪と補助輪系をアナステックで遊ぶ。(ESP-NOW)

注意

• 過去ログを見よ!!!
• RCサーボーモータは、SG90を使用
• 両方ともM5NanoC6
• 自己責任の意味がわかる人
• ESP32 Arduino 3.1.3
• あまり正確では、ない
• 10ビット(1024)の固定小数点の処理をしている
• おどしでも、冗談でもなく、サーボ系は、一発で終わるので、ちょとめんどい。(悪い意味)
• サーボをパソコンに繋いだままやるとパソコンが破壊されます。
• モーター、コイル等、逆起電力で高電圧が発生する時の安全対策は、各自実行してください。
• 最低でも、試す時は、モバイルバッテリを使用してください。
• モータ系の力学装置系をなぜ、あまりやらないのかは、まず、物理的にお金がかかる、いろいろ、危険であるから

結果

プログラム

//ESP32_NOW_m_analog_joystick_7_M5NanoC6_1
//1対1に改造する　対応
//ブロードキャスト
//最小二乗法
//0点の改善(20250225)

//インクルド (ESP-NOW)
#include <Arduino.h>
#include <string.h>     //memcpy
#include "ESP32_NOW.h"
#include "WiFi.h"
#include <esp_mac.h>  // For the MAC2STR and MACSTR macros


//定義

/* Definitions */
//WiFiチャンネルの指定
#define ESPNOW_WIFI_CHANNEL 1

//クラス
/* Classes */

// Creating a new class that inherits from the ESP_NOW_Peer class is required.

class ESP_NOW_Broadcast_Peer : public ESP_NOW_Peer {
public:
  // Constructor of the class using the broadcast address
  ESP_NOW_Broadcast_Peer(const uint8_t *mac_addr, uint8_t channel, wifi_interface_t iface, const uint8_t *lmk)
    : ESP_NOW_Peer(mac_addr, channel, iface, lmk) {}

  // Destructor of the class
  ~ESP_NOW_Broadcast_Peer() {
    remove();
  }

  // Function to properly initialize the ESP-NOW and register the broadcast peer
  bool begin() {
    if (!ESP_NOW.begin() || !add()) {
      log_e("Failed to initialize ESP-NOW or register the broadcast peer");
      return false;
    }
    return true;
  }

  // Function to send a message to all devices within the network
  bool send_message(const uint8_t *data, size_t len) {
    if (!send(data, len)) {
      log_e("Failed to broadcast message");
      return false;
    }
    return true;
  }
};

/* Global Variables */

//ブロードキャスト オブジェクトの定義
// Create a broadcast peer object
ESP_NOW_Broadcast_Peer broadcast_peer(ESP_NOW.BROADCAST_ADDR, ESPNOW_WIFI_CHANNEL, WIFI_IF_STA, NULL);

//const MacAddress peer_mac({0x48, 0x27, 0xe2, 0xe3, 0xbd, 0x50});
//ESP_NOW_Broadcast_Peer broadcast_peer(  peer_mac  , ESPNOW_WIFI_CHANNEL, WIFI_IF_STA, NULL);


/* Main */

#define CP (uint8_t *)
#define G_X 2
#define G_Y 1

float o_yl = 0; //L側の0(ゼロ)点
float o_yr = 0; //R側の0(ゼロ)点


//初期化
void setup() {

  //WiFiの初期化
  // Initialize the Wi-Fi module
  WiFi.mode(WIFI_STA);
  WiFi.setChannel(ESPNOW_WIFI_CHANNEL);
  delay(3000);  //決め打ちなので、おかしかったら調整してね!

//  //シリアルの初期化 //debug(1)
//  Serial.begin(9600);
//  Serial.println();
//  //シリアルの待ちが0.5*9
//  for (int i = 0; i < 9; i++) {
//    Serial.print('.'); delay(500); //接続待ち
//  } //for
//  Serial.println();
  
  //ブロードキャストの初期化
  // Register the broadcast peer
  if (!broadcast_peer.begin()) {
    delay(5000);    //瞬リセット防止用
    ESP.restart();  //リセット
  }                 //endif

  //テストメッセージを送る
  char pp[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
  delay(5);
  broadcast_peer.send_message(CP(pp), 8 + 0);
  delay(5);
  broadcast_peer.send_message(CP(pp), 8 + 0);
  delay(5);
  broadcast_peer.send_message(CP(pp), 8 + 0);
  delay(5);

  //アナログジョイステックの初期化と0点の設定
  //gpio inte
  pinMode(G_X, ANALOG);  //gpio init
  pinMode(G_Y, ANALOG);  //gpio init

  //R START
  int s = 0;   //input
  for (int i = 0; i < 65536; i++) {
    //センサー入力する
    s = s + analogRead(G_X); //センサーの値
  } //for
  s = s * 10;
  s = s / 65536;
  o_yl = ((float)s) * 0.0001;

  //R START
  s = 0;
  for (int i = 0; i < 65536; i++) {
    //センサー入力する
    s = s + analogRead(G_Y); //センサーの値
  } //for
  s = s * 10;
  s = s / 65536;
  o_yr = ((float)s) * 0.0001;

//  //debug 0点の値を無線電送
//  struct {
//    float yl; //L側
//    float yr; //R側
//  } debug_y;
//
//  debug_y.yl = o_yl;
//  debug_y.yr = o_yr;
//
//  //int (32bit) を byte に分割する処理
//  unsigned char *p;
//  p = (unsigned char *)(&debug_y);
//  memcpy(pp, p, sizeof(debug_y)); 
//
//  //ESP-NOWのパケット(フレーム)を送信する
//  broadcast_peer.send_message(CP(pp), sizeof(debug_y));
//  delay(5);
//
//  delay(3000);

}  //setup


//送信バッファー int(32bit)
//グローバルで定義しているのは、速度低下を防ぐため
unsigned char ac[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };

struct {
  float yl; //L側
  float yr; //R側
} Ys;

//メインループ
void loop() {

  int o_n[20]; //一時配列
  int s;  //input
  int xydie = 0; //x偏差とy偏差
  int b; //傾き
  int a; //初期位置

  //llllllllllllllllllllllllllllllll START
  s = 0;
  for (int i = 1; i <= 15; i++) {
    //センサー入力する
    s = s + (o_n[i] = (analogRead(G_X) * 1000)); //センサーの値
  } //for
  s = s / 15; // s / 15

  xydie = 0;
  for (int i = 1; i <= 15; i++) {
    xydie = xydie + (   (i - 8) *  (o_n[i] - s)  );
    //Serial.printf("%d,",(i - 8) *  (o_n[i] - s)  );
  } //for

  b = xydie / 280;
  //int a = s -(xydie / 280)*8;
  a = s - ((xydie * 8) / 280);

  //Serial.printf("\n");
  //Serial.printf("xydie = %d\n", xydie);
  //Serial.printf("\n");
  //Serial.printf("b = %d\n", b);
  //Serial.printf("a = %d\n", a);

  //c_yl = ((float)(a+b*15)) * (3.3f / 4096.0f);  //  電圧こと
  //c_yl = ((float)(a+((xydie*15) / 280))) * (3.3f / 4096.0f);  //  電圧こと
  Ys.yl = ((float)(a + ((xydie * 15) / 280))) * 0.000001f; //  電圧こと  x0.000001 = /1000000
  Ys.yl = Ys.yl - o_yl;

  //表示
  //Serial.printf("L=%f\n", Ys.yl);
  //Serial.println();
  //llllllllllllllllllllllllllllllll END

  //RRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRR START
  s = 0;
  for (int i = 1; i <= 15; i++) {
    //センサー入力する
    s = s + (o_n[i] = (analogRead(G_Y) * 1000)); //センサーの値
  } //for
  s = s / 15; // s / 15

  xydie = 0;
  for (int i = 1; i <= 15; i++) {
    xydie = xydie + (   (i - 8) *  (o_n[i] - s)  );
    //Serial.printf("%d,",(i - 8) *  (o_n[i] - s)  );
  } //for

  b = xydie / 280;
  //int a = s -(xydie / 280)*8;
  a = s - ((xydie * 8) / 280);

  //Serial.printf("\n");
  //Serial.printf("xydie = %d\n", xydie);
  //Serial.printf("\n");
  //Serial.printf("b = %d\n", b);
  //Serial.printf("a = %d\n", a);

  //c_yr = ((float)(a+b*15)) * (3.3f / 4096.0f);  //  電圧こと
  //c_yr = ((float)(a+((xydie*15) / 280))) * (3.3f / 4096.0f);  //  電圧こと
  Ys.yr = ((float)(a + ((xydie * 15) / 280))) * 0.000001f; //  電圧こと  x0.000001 = /1000000
  Ys.yr = Ys.yr - o_yr;

  //表示
  //Serial.printf("R=%f\n", Ys.yr);
  //Serial.println();
  //RRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRR END

  //int (32bit) を byte に分割する処理
  unsigned char *p;
  p = (unsigned char *)(&Ys);
  memcpy(ac, p, sizeof(Ys)); 

  //ESP-NOWのパケット(フレーム)を送信する
  broadcast_peer.send_message(CP(ac), sizeof(Ys));
  delay(5);

  //Serial.printf("%f,%f\n", Ys.yl,Ys.yr); //debug(1)
  
  delay(300);  //0.3秒待つ

}  //loop

//ESP32_NOW_s_analog_joystick_7_M5NanoC6_1
//1対1に改造する　対応
//ブロードキャスト
//いちお、割り込み時の事を考えた
//主に表示の桁揃えをした(20250225)


/*
    ESP-NOW Broadcast Slave
    Lucas Saavedra Vaz - 2024

    This sketch demonstrates how to receive broadcast messages from a master device using the ESP-NOW protocol.

    The master device will broadcast a message every 5 seconds to all devices within the network.

    The slave devices will receive the broadcasted messages. If they are not from a known master, they will be registered as a new master
    using a callback function.
*/


//インクルド
#include <Arduino.h>
#include <string.h>    //memcpy
#include "ESP32_NOW.h"
#include "WiFi.h"
#include <esp_mac.h>  // For the MAC2STR and MACSTR macros
#include <vector>
#include "MacAddress.h"


//定義
struct {
  float yl; //左側のy
  float yr; //右側のy
} Ys;

static volatile int s_p = 0; //割り込み処理中フラグ

//使い方　SOのマクロに0から180を設定すると
//PWMの間隔の値が返る
// a = SO(i);
// //a = SO(180-i);
//V_0が0度の時の初期値で単位は、nS(ナノ秒)
//V_180が180度の時の初期値で単位は、nS(ナノ秒)
//SO(角度) 正転
//M_SO(角度)　逆転
#define V_0    500
#define V_180 2400
#define Wavelength_1count (((1000000/50)*1024)/4096)
#define T_MIN ((V_0*1024)/Wavelength_1count)
#define T_MAX ((V_180*1024)/Wavelength_1count)
#define HH  ((T_MAX-T_MIN)*1024)/180
#define SO(AA) (T_MIN+(((AA)*(HH))>>10))
#define M_SO(AA) (SO(180-(AA)))

//PWMのポート、周期、分解度のビット数
#define PWM_PIN_A 1
#define PWM_PIN_B 2
#define FREQUENCY 50
#define RESOLUTION 12

/* Definitions */

#define ESPNOW_WIFI_CHANNEL 1

//プロトタイプ宣言
int acceleration_num();
void acceleration_write(unsigned char *a);
void acceleration_read(unsigned char *a);


//クラス
/* Classes */

// Creating a new class that inherits from the ESP_NOW_Peer class is required.

class ESP_NOW_Peer_Class : public ESP_NOW_Peer {
  public:
    // Constructor of the class
    ESP_NOW_Peer_Class(const uint8_t *mac_addr, uint8_t channel, wifi_interface_t iface, const uint8_t *lmk)
      : ESP_NOW_Peer(mac_addr, channel, iface, lmk) {}

    // Destructor of the class
    ~ESP_NOW_Peer_Class() {}

    // Function to register the master peer
    bool add_peer() {
      if (!add()) {
        log_e("Failed to register the broadcast peer");
        return false;
      }
      return true;
    }

    // Function to print the received messages from the master
    void onReceive(const uint8_t *data, size_t len, bool broadcast) {

      s_p = 1;

      //読み込みデータが8バイトの時
      if (len == sizeof(Ys)) {

        //読み込みデータをセンサーの値に合成する
        unsigned char *p;
        p = (unsigned char *)(&Ys);
        memcpy(p, data, sizeof(Ys)); //センサーの値をセット

      }  //if

      //Serial.printf("Received a message from master " MACSTR " (%s)\n", MAC2STR(addr()), broadcast ? "broadcast" : "unicast");
      //Serial.printf("  Message: %s\n", (char *)data);

      s_p = 0;

    }
};

/* Global Variables */

// List of all the masters. It will be populated when a new master is registered
std::vector<ESP_NOW_Peer_Class> masters;

/* Callbacks */

// Callback called when an unknown peer sends a message
void register_new_master(const esp_now_recv_info_t *info, const uint8_t *data, int len, void *arg) {

  //自分
  const MacAddress peer_mac1( { 0x48, 0x27, 0xe2, 0xe3, 0xbd, 0x50 });
  unsigned char a_add[6] = { 0x48, 0x27, 0xe2, 0xe3, 0xbd, 0x50 };
  //相手
  const MacAddress peer_mac2( { 0x40, 0x4c, 0xca, 0x5a, 0xe3, 0x1c });

  if (memcmp(info->des_addr, ESP_NOW.BROADCAST_ADDR, 6) == 0) {
    //if (memcmp(info->des_addr, a_add, 6) == 0) {
    Serial.printf("Unknown peer " MACSTR " sent a broadcast message\n", MAC2STR(info->src_addr));
    Serial.println("Registering the peer as a master");

    ESP_NOW_Peer_Class new_master(info->src_addr, ESPNOW_WIFI_CHANNEL, WIFI_IF_STA, NULL);

    masters.push_back(new_master);
    if (!masters.back().add_peer()) {
      Serial.println("Failed to register the new master");
      return;
    }
  } else {
    // The slave will only receive broadcast messages
    log_v("Received a unicast message from " MACSTR, MAC2STR(info->src_addr));
    log_v("Igorning the message");
  }
}


/* Main */

//初期化
void setup() {

  //出力ピンの初期化
  pinMode(PWM_PIN_A, OUTPUT);
  pinMode(PWM_PIN_B, OUTPUT);
    
  // PWM設定（ピン番号, 周波数, 分解能）
  ledcAttach(PWM_PIN_A, FREQUENCY, RESOLUTION);
  ledcAttach(PWM_PIN_B, FREQUENCY, RESOLUTION);

  //WiFiの初期化
  // Initialize the Wi-Fi module
  WiFi.mode(WIFI_STA);
  WiFi.setChannel(ESPNOW_WIFI_CHANNEL);

  //シリアルの初期化
  Serial.begin(9600);
  Serial.println();
  //シリアルの待ちが0.5*9
  //delay(3000);//決め打ちなので、おかしかったら調整してね! wifiの待ち0.5*6
  for (int i = 0; i < (9 + 6); i++) {
    delay(500);  //接続待ち
    Serial.print('.');
  }  //for
  Serial.println();
  delay(500);

  //アドレスとWiFiチャンネルの表示
  Serial.println("ESP-NOW Example - Broadcast Slave");
  Serial.println("Wi-Fi parameters:");
  Serial.println("  Mode: STA");
  Serial.println("  MAC Address: " + WiFi.macAddress());
  Serial.printf("  Channel: %d\n", ESPNOW_WIFI_CHANNEL);

  //ブロードキャストの初期化
  // Initialize the ESP-NOW protocol
  if (!ESP_NOW.begin()) {
    Serial.println("Failed to initialize ESP-NOW");
    Serial.println("Reeboting in 5 seconds...");
    delay(5000);    //瞬リセット防止用
    ESP.restart();  //リセット
  }                 //endif

  //ESP-NOWのコールバック(割り込み)
  // Register the new peer callback
  ESP_NOW.onNewPeer(register_new_master, NULL);

  //スタートが成功したと表示
  Serial.println("Setup complete. Waiting for a master to broadcast a message...");

}  //setup


//メインループ
void loop() {

  //表示
  while (s_p) {} //書き換え中の時
  Serial.printf("%9.6f", Ys.yl);
  Serial.print(',');
  while (s_p) {} //書き換え中の時
  Serial.printf("%9.6f", Ys.yr);
  Serial.println();

  //Serial.printf("Received a message from master " MACSTR " (%s)\n", MAC2STR(addr()), broadcast ? "broadcast" : "unicast");
  //Serial.printf("  Message: %s\n", (char *)data);

  float f; //一時

  // DUTY比を指定して、ポートに対して実行
  while (s_p) {} //書き換え中の時
  f = Ys.yl;
  f = f * (90.0f /1.650f );
  f = f + 90.0f;
  if(f > 180.0f) {f = 180.0f;}
  if(f < 0.0f) {f = 0.0f;}
  Serial.printf("A%9.6f\n", f);
  ledcWrite(PWM_PIN_A, M_SO(((int)f)));

  // DUTY比を指定して、ポートに対して実行
  while (s_p) {} //書き換え中の時
  f = Ys.yr;
  f = f * (90.0f /1.650f );
  f = f + 90.0f;
  if(f > 180.0f) {f = 180.0f;}
  if(f < 0.0f) {f = 0.0f;}
  Serial.printf("B%9.6f\n", f);
  ledcWrite(PWM_PIN_B, M_SO(((int)f)));

  delay(300);  //ダミー 0.3秒待つ

}  //loop