3x Arduino + ESP32 with ESP-NOW - c++

Question

The configuration on "machine" looks like this:
Computer with custom app WPF C#, have connected ESP32-sender.
On other side of room there is ESP32 as recaiver that have connected to it 3 arduino due, that controls a lot of stepper motors.
My problem is, that if I send a single command all works great. Directly on other side it executes.
The problem starts when there is a lot of commands to send. ESP32 starts to makes Delivery fail status and after that moment all further commands are lost.
That's the code of Master-ESP connected to PC.
Which arduino to send is decide by one of the symbols on begin of message "!,#,#"
#include <esp_now.h>
#include <WiFi.h>
#include <Wire.h>
uint8_t broadcastAddress[] = {0x94, 0xB9, 0x7E, 0xD0, 0x93, 0x64};
String inMess;
typedef struct Message {
char a[100];
} Message;
// Variable to store if sending data was successful
String success;
Message message;
Message send_message;
esp_now_peer_info_t peerInfo;
char incoming;
String full = "";
bool text_done = false;
bool sended = false;
// Callback when data is sent
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
//Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success*" : "Delivery Fail*");
if (status ==0){
success = "Delivery Success :)";
text_done = false;
full = "";
sended = false;
}
else{
success = "Delivery Fail :(";
delay(10);
sended = false;
Serial.println(success);
}
}
// Callback when data is received
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
memcpy(&message, incomingData, sizeof(message));
Serial.println(message.a);
}
void setup() {
// Init Serial Monitor
Serial.begin(115200);
// Set device as a Wi-Fi Station
WiFi.mode(WIFI_STA);
Serial.println(WiFi.macAddress());
// Init ESP-NOW
if (esp_now_init() != ESP_OK) {
Serial.println("Error initializing ESP-NOW");
return;
}
esp_now_register_send_cb(OnDataSent);
// Register peer
memcpy(peerInfo.peer_addr, broadcastAddress, 6);
peerInfo.channel = 0;
peerInfo.encrypt = false;
// Add peer
if (esp_now_add_peer(&peerInfo) != ESP_OK){
Serial.println("Failed to add peer");
return;
}
esp_now_register_recv_cb(OnDataRecv);
}
void loop() {
if(Serial.available() > 0 && !text_done){
incoming = Serial.read();
if(incoming == '\n'){
text_done = true;
full.trim();
full.toUpperCase();
}
if(text_done == false){
full += incoming;
}
}
if(text_done){
if(full[0] != '!' && full[0] != '#' && full[0] != '#'){ //check if text is worth sending to other esp
text_done = false;
full = "";
}
}
if(text_done){
if(!sended){
full.toCharArray(send_message.a,sizeof(send_message));
esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &send_message, sizeof(send_message));
if (result == ESP_OK) {
//Serial.println("Sent success*");
sended = true;
}
else {
Serial.println("Error sending*");
delay(10);
}
}
}
}
That's the code of recaiver ESP
#include <esp_now.h>
#include <WiFi.h>
#include <SoftwareSerial.h>
#include <Wire.h>
SoftwareSerial worker;
SoftwareSerial tool;
//Serial2 is Table, software serials are others
uint8_t broadcastAddress[] = {0x7C, 0x9E, 0xBD, 0x4B, 0x47, 0xA4};
String outMess;
String outMessTable;
String outMessTool;
String inMess;
typedef struct Message {
char a[100];
} Message;
String success;
Message message;
Message send_message;
bool sended = true;
String again_message = "";
esp_now_peer_info_t peerInfo;
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
//Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
if (status == 0) {
success = "Delivery Success :)";
sended = true;
again_message = "";
}
else {
success = "Delivery Fail :(";
sended = false;
delay(5);
}
}
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
memcpy(&message, incomingData, sizeof(message));
Serial.println(message.a);
sendTo((String)message.a);
}
void setup() {
// Init Serial Monitor
Serial.begin(115200);
Serial2.begin(115200, SERIAL_8N1, 16, 17);
//rx tx
worker.begin(57600, SWSERIAL_8N1, 25, 26, false);
if (!worker) {
Serial.println("Invalid SoftwareSerial pin configuration, check config");
while (1) {
delay (1000);
}
}
//rx tx
tool.begin(57600, SWSERIAL_8N1, 32, 33, false);
if (!tool) {
Serial.println("Invalid SoftwareSerial pin configuration, check config");
while (1) {
delay (1000);
}
}
// Set device as a Wi-Fi Station
WiFi.mode(WIFI_STA);
Serial.println(WiFi.macAddress());
// Init ESP-NOW
if (esp_now_init() != ESP_OK) {
Serial.println("Error initializing ESP-NOW");
return;
}
// Once ESPNow is successfully Init, we will register for Send CB to
// get the status of Trasnmitted packet
esp_now_register_send_cb(OnDataSent);
// Register peer
memcpy(peerInfo.peer_addr, broadcastAddress, 6);
peerInfo.channel = 0;
peerInfo.encrypt = false;
// Add peer
if (esp_now_add_peer(&peerInfo) != ESP_OK) {
Serial.println("Failed to add peer");
return;
}
// Register for a callback function that will be called when data is received
esp_now_register_recv_cb(OnDataRecv);
}
void loop() {
// Set values to send
if(sended){
if (worker.available() > 0) {//!
char t = worker.read();
if(t == '\n'){
outMess.trim();
sendToEsp(outMess,"!");
outMess = "";
}
else{
outMess += t;
}
}
}else{
if(again_message.length()>0){
delay(10);
sendToEsp(again_message.substring(1),again_message.substring(0,1));
}else{
sended = true;
}
}
if(sended){
if (tool.available() > 0) {//#
char t = tool.read();
if(t == '\n'){
outMessTool.trim();
sendToEsp(outMessTool,"#");
outMessTool = "";
}
else{
outMessTool += t;
}
}
}else{
if(again_message.length()>0){
delay(10);
sendToEsp(again_message.substring(1),again_message.substring(0,1));
}else{
sended = true;
}
}
if(sended){
if (Serial2.available() > 0) { //#
char t = Serial2.read();
if(t == '\n'){
outMessTable.trim();
sendToEsp(outMessTable,"#");
outMessTable = "";
}else{
outMessTable += t;
}
}
}else{
if(again_message.length()>0){
delay(10);
sendToEsp(again_message.substring(1),again_message.substring(0,1));
}else{
sended = true;
}
}
if(Serial.available() > 0){
outMess = Serial.readStringUntil('\n'); //read command from pc
outMess.trim(); // remove uneccesery spaces
sendTo(outMess);
}
}
void sendToEsp(String text, String which){
String mess = which + text;
again_message = mess;
mess.toCharArray(send_message.a,sizeof(send_message));
esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &send_message, sizeof(send_message));
if (result == ESP_OK) {
// Serial.println("Sent with success");
}
else {
Serial.println("Error sending the data");
sended = true;
again_message = "";
}
}
void sendTo(String outMess){
Serial.print("=");
Serial.print(outMess);
if(outMess[0] == '!'){ //worker
worker.enableTx(true);
worker.println(outMess.substring(1));
worker.enableTx(false);
Serial.println("send to worker");
}
if(outMess[0] == '#') {//table
Serial2.println(outMess.substring(1));
Serial.println("send to table");
}
if(outMess[0] == '#'){ //tool
tool.enableTx(true);
tool.println(outMess.substring(1));
tool.enableTx(false);
Serial.println("send to tool");
}
}
If I send one message with a delay of writing next one by hand it works great.
If the message is sended via c# very fast, esp32 is handling first few of them then losing on sending or recaiving, sometimes both of esp32 sometimes only one.
How could I prevent that to make stable connection ?