#include <esp_now.h>
#include <WiFi.h>
#include <Wire.h>
// Global copy of slave
esp_now_peer_info_t slave;
#define CHANNEL 3
#define PRINTSCANRESULTS 0
#define DELETEBEFOREPAIR 0
const byte accelerometerAddress = 0x68;
bool isPaired = false;
typedef struct struct_message{
float AccX;
float AccY;
float AccZ;
} struct_message;
// Init ESP Now with fallback
void InitESPNow() {
WiFi.disconnect();
if (esp_now_init() == ESP_OK) {
Serial.println("ESPNow Init Success");
}
else {
Serial.println("ESPNow Init Failed");
// Retry InitESPNow, add a counte and then restart?
// InitESPNow();
// or Simply Restart
ESP.restart();
}
}
// Scan for slaves in AP mode
void ScanForSlave() {
int8_t scanResults = WiFi.scanNetworks();
// reset on each scan
bool slaveFound = 0;
memset(&slave, 0, sizeof(slave));
Serial.println("");
if (scanResults == 0) {
Serial.println("No WiFi devices in AP Mode found");
} else {
Serial.print("Found "); Serial.print(scanResults); Serial.println(" devices ");
for (int i = 0; i < scanResults; ++i) {
// Print SSID and RSSI for each device found
String SSID = WiFi.SSID(i);
int32_t RSSI = WiFi.RSSI(i);
String BSSIDstr = WiFi.BSSIDstr(i);
if (PRINTSCANRESULTS) {
Serial.print(i + 1);
Serial.print(": ");
Serial.print(SSID);
Serial.print(" (");
Serial.print(RSSI);
Serial.print(")");
Serial.println("");
}
delay(10);
// Check if the current device starts with `BaseStation`
if (SSID.indexOf("BaseStation") == 0) {
// SSID of interest
Serial.println("Found a BaseStation.");
Serial.print(i + 1); Serial.print(": "); Serial.print(SSID); Serial.print(" ["); Serial.print(BSSIDstr); Serial.print("]"); Serial.print(" ("); Serial.print(RSSI); Serial.print(")"); Serial.println("");
// Get BSSID => Mac Address of the Slave
int mac[6];
if ( 6 == sscanf(BSSIDstr.c_str(), "%x:%x:%x:%x:%x:%x", &mac[0], &mac[1], &mac[2], &mac[3], &mac[4], &mac[5] ) ) {
for (int ii = 0; ii < 6; ++ii ) {
slave.peer_addr[ii] = (uint8_t) mac[ii];
}
}
slave.channel = CHANNEL; // pick a channel
slave.encrypt = 0; // no encryption
slaveFound = 1;
// we are planning to have only one slave in this example;
// Hence, break after we find one, to be a bit efficient
break;
}
}
}
if (slaveFound) {
Serial.println("BaseStation Found, processing..");
} else {
Serial.println("BaseStation Not Found, trying again.");
}
// clean up ram
WiFi.scanDelete();
}
// Check if the slave is already paired with the master.
// If not, pair the slave with master
bool manageSlave() {
if (slave.channel == CHANNEL) {
if (DELETEBEFOREPAIR) {
deletePeer();
}
Serial.print("BaseStation Status: ");
// check if the peer exists
bool exists = esp_now_is_peer_exist(slave.peer_addr);
if ( exists) {
// Slave already paired.
Serial.println("Already Paired");
return true;
} else {
// Slave not paired, attempt pair
esp_err_t addStatus = esp_now_add_peer(&slave);
if (addStatus == ESP_OK) {
// Pair success
Serial.println("Pair success");
return true;
} else if (addStatus == ESP_ERR_ESPNOW_NOT_INIT) {
// How did we get so far!!
Serial.println("ESPNOW Not Init");
return false;
} else if (addStatus == ESP_ERR_ESPNOW_ARG) {
Serial.println("Invalid Argument");
return false;
} else if (addStatus == ESP_ERR_ESPNOW_FULL) {
Serial.println("Peer list full");
return false;
} else if (addStatus == ESP_ERR_ESPNOW_NO_MEM) {
Serial.println("Out of memory");
return false;
} else if (addStatus == ESP_ERR_ESPNOW_EXIST) {
Serial.println("Peer Exists");
return true;
} else {
Serial.println("Not sure what happened");
return false;
}
}
} else {
// No slave found to process
Serial.println("No BaseStation found to process");
return false;
}
}
void deletePeer() {
esp_err_t delStatus = esp_now_del_peer(slave.peer_addr);
Serial.print("BaseStation Delete Status: ");
if (delStatus == ESP_OK) {
// Delete success
Serial.println("Success");
} else if (delStatus == ESP_ERR_ESPNOW_NOT_INIT) {
// How did we get so far!!
Serial.println("ESPNOW Not Init");
} else if (delStatus == ESP_ERR_ESPNOW_ARG) {
Serial.println("Invalid Argument");
} else if (delStatus == ESP_ERR_ESPNOW_NOT_FOUND) {
Serial.println("Peer not found.");
} else {
Serial.println("Not sure what happened");
}
}
// send data
void sendData(struct_message data) {
const uint8_t *peer_addr = slave.peer_addr;
Serial.print("Sending: "); Serial.print(data.AccX); Serial.print(" "); Serial.print(data.AccY); Serial.print(" "); Serial.println(data.AccZ);
esp_err_t result = esp_now_send(peer_addr, (uint8_t *) &data, sizeof(data));
Serial.print("Send Status: ");
if (result == ESP_OK) {
Serial.println("Success");
} else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
// How did we get so far!!
Serial.println("ESPNOW not Init.");
} else if (result == ESP_ERR_ESPNOW_ARG) {
Serial.println("Invalid Argument");
} else if (result == ESP_ERR_ESPNOW_INTERNAL) {
Serial.println("Internal Error");
} else if (result == ESP_ERR_ESPNOW_NO_MEM) {
Serial.println("ESP_ERR_ESPNOW_NO_MEM");
} else if (result == ESP_ERR_ESPNOW_NOT_FOUND) {
Serial.println("Peer not found.");
} else {
Serial.println("Not sure what happened");
}
}
// callback when data is sent from Master to Slave
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");
if (status != ESP_NOW_SEND_SUCCESS) {
isPaired = false;
}
}
struct_message readAccelerometer () {
struct_message data;
Wire.beginTransmission(accelerometerAddress);
Wire.write(0x3B); // start at ACCEL_XOUT_H
Wire.endTransmission(false);
// request registers ACCEL_XOUT_H through ACCEL_ZOUT_L
Wire.requestFrom(accelerometerAddress, 6, true); // request 6 bytes
// read data into memory and scale to g's
int16_t accelerometerX, accelerometerY, accelerometerZ;
if (Wire.available() == 6) {
accelerometerX = (Wire.read() << 8 | Wire.read());
data.AccX = accelerometerX / 16384.0; // 8129.0 was reading 1g as 2g
accelerometerY = (Wire.read() << 8 | Wire.read());
data.AccY = accelerometerY / 16384.0;
accelerometerZ = (Wire.read() << 8 | Wire.read());
data.AccZ = accelerometerZ / 16384.0;
}
Wire.endTransmission(true);
return data;
}
void setup() {
Serial.begin(115200);
//Set device in STA mode to begin with
WiFi.mode(WIFI_STA);
Serial.println("ESPNow/Basic/Master Example");
// This is the mac address of the Master in Station Mode
Serial.print("STA MAC: "); Serial.println(WiFi.macAddress());
// Init ESPNow with a fallback logic
InitESPNow();
// Once ESPNow is successfully Init, we will register for Send CB to
// get the status of Trasnmitted packet
esp_now_register_send_cb(OnDataSent);
// configure accelerometer
Wire.begin();
Wire.beginTransmission(accelerometerAddress);
Wire.write(0x1B); // select GYRO_CONFIG
Wire.write(0x08); // write 00001000, no self test, +/- 4g
Wire.endTransmission();
Wire.beginTransmission(accelerometerAddress);
Wire.write(0x6B); // select PWR_MGMT_1
Wire.write(0x00); // write 000000000, turn off sleep mode use 8MHz oscilator
Wire.endTransmission();
}
void loop() {
while (!isPaired) { // not connected search untill found
// In the loop we scan for slave
ScanForSlave();
// If Slave is found, it would be populate in `slave` variable
// We will check if `slave` is defined and then we proceed further
if (slave.channel == CHANNEL) { // check if slave channel is defined
// `slave` is defined
// Add slave as peer if it has not been added already
isPaired = manageSlave();
if (!isPaired) {
// slave pair failed
Serial.println("Slave pair failed!");
}
}
else {
// No slave found to process
}
// wait for 3seconds to run the logic again
delay(3000);
}
// connected
Serial.println("Connected!");
sendData(readAccelerometer());
delay(50);
}