SCP 同期実験　ESP-NOW Slave Program M5Atomlite

SCP_slave_M5ATOM_ESP-NOW_2port_dataStr_rev04.ino

#include <M5StickC.h>
//#include <M5Stack.h>
#include <esp_now.h>
#include <WiFi.h>
int i,j,k,n,m;
int t,t_1;
volatile int stime=0,stime_1=0;

volatile int  rt0=0,rt1=0,rt2=0;
esp_now_peer_info_t slave;
 //volatile int value0,value1,value2,value3;
 volatile int val[20];
  uint8_t cdata[6]={};
// 送信コールバック
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  char macStr[18];
  snprintf(macStr, sizeof(macStr), "%02X:%02X:%02X:%02X:%02X:%02X",
           mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  //Serial.print("Last Packet Sent to: ");
  //Serial.println(macStr);
  //Serial.print("Last Packet Send Status: ");
  //Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
 
  
}
//**************************************************************************************
// ===============受信コールバック関数で全部受信============================================
//*******************************************************************************************
void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len) {
  char macStr[18];
  snprintf(macStr, sizeof(macStr), "%02X:%02X:%02X:%02X:%02X:%02X",
           mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
//=========================Data align========================================
// SPC SingleAxis data: data[0]=Force,data[1]=accX,data[2]=accY,data[3]=accZ,data[4]=sdata
//Dummy 5Axis data: data[0]=FP1,data[1]=Fx,data[2]=Fy,data[3]=Fz.data[4]=FP3,data[5]=FP2,data[6]=sdata
//val[0]=Force,val[1]=accX,val[2]=accY,val[3]=accZ,val[4]=sdata,
//,val[5]=FP1,val[6]=Fx,val[7]=Fy,val[8]=Fz,val[9]=FP3,val[10]=FP2,val[11]=sdata
int strNo=0;
int sdata_1=0;
String dstr[12]={};
//Serial.println(mac_addr[5],HEX);
if(mac_addr[5]==0xCC){strNo=0; }//SPC_SingleAxis 5data
if(mac_addr[5]==0x2C){strNo=5; }//Dummy 5Axis 7data
//====binary 4byte data to integer 4byte data======================
 int datanum=int(data_len/4);
 for (i=0;i<datanum;i++)
      {
          val[i+strNo]=data[i*4]|data[i*4+1]<<8|data[i*4+2]<<16|data[i*4+3]<<24;//Left Shift  OR
          //Serial.printf("val[%d]=%d\n\r",i+strNo,val[i+strNo]);
         
      }
//Serial.printf("val[4]=%d,val[11]=%d\n\r",val[4],val[11]);

if(val[4]==val[11] && val[4]==stime && stime-stime_1>0)
{
  Serial.printf("%d,%d,%d,%d,%d,%d,%d,%d\n\r",val[0],val[1],val[5],val[6],val[4],val[11],stime,rt0);
  stime_1=stime;
}
     
} 
//*************************************************************************************************
//+++++++++++++++i_to_char+++++++++++++++++
// i=IntegerValueData,*d=Array pointer, n=Array start No
void  i_to_char(int i, uint8_t *d, int n)
{
  d[n] = i & 0x000000ff;
  d[n + 1] = (i & 0x0000ff00) >> 8;
  d[n + 2] = (i & 0x00ff0000) >> 16;
  d[n + 3] = (i & 0xff000000) >> 24;
}
//++++++++++++++++++++++++++++++++++++++++++  
void setup() {
  M5.begin();
  
  // ESP-NOW初期化
  WiFi.mode(WIFI_STA);
  WiFi.disconnect();
  if (esp_now_init() == ESP_OK) {
    Serial.println("ESPNow Init Success");
     
  } else {
    Serial.println("ESPNow Init Failed");
     
    ESP.restart();
  }
 
  // マルチキャスト用Slave登録
  memset(&slave, 0, sizeof(slave));
  for (int i = 0; i < 6; ++i) {
    slave.peer_addr[i] = (uint8_t)0xff;
  }
  esp_err_t addStatus = esp_now_add_peer(&slave);
  if (addStatus == ESP_OK) {
    // Pair success
    Serial.println("Pair success");
  }
 
  // ESP-NOWコールバック登録
  esp_now_register_send_cb(OnDataSent);
  esp_now_register_recv_cb(OnDataRecv);
  n=0;//douki couter
}
 
void loop() {
  M5.update();
     t=micros();
  // ボタンを押したら送信
  //if ( M5.BtnB.wasPressed() ) {//For Atom HomeButton=GPIO39=> M5.BtnB.wasPressed()   For M5StickC GPIO37=> M5.BtnA.wasPressed()
    if(t-t_1>20000)
    {
      t_1=micros();
      
      stime++;
      cdata[0] =0x73;//"s" 
      cdata[1] =0x74;//"t" //start command
      i_to_char(stime,cdata,3);//n=>cdata[3][4][5][6]
      rt0=millis();
    esp_err_t result = esp_now_send(slave.peer_addr, cdata, sizeof(cdata));
    //Serial.printf("============stime=%d===================\n\r",stime);
    }
  delay(1);
}