Servo not working over ESPNOW

Projects Programming
1

I'm working on a robot controlled by a remote via ESPNOW (two way) protocol. The robot has three stepper motor,1 dc motor and 1 servo motor. The problem is servo is not working even if the values (jaw) for it are being transmitted by the remote. below are the codes for both the remote and robot:

Remote:


//--------------------------------------------
//ESP32 remote via ESP-NOW Protocol
//Transmit joy vals- fsr val
//--------------------------------------------
#include <esp_now.h>
#include <WiFi.h>
#include <ESP32MX1508.h>

//------------------------------------------------------------
#define open_pin 12

//FEEDBACK MOTOR
#define ROTA 4
#define ROTB 15
#define CH1 0
#define CH2 1
#define RES 12

MX1508 motorA(ROTA, ROTB, CH1, CH2, RES); //object for yrot motor

int motorSpeedMin = 0;
int motorSpeedMax = 4095;

// Define FSR threshold and motor speed limits
const int fsrThreshold = 500;  // Adjust thresho
int fsrVal = 0;



//// Yrotation button pins
#define CW_BUTTON_PIN  25
#define ACW_BUTTON_PIN  26

//joystick pins
#define x  32
#define y  33
#define z 34
//#define y2 35


//STATES
#define FWD     2
#define REV     1
#define STOP    0
#define OPEN    0
#define CLOSE   1
#define CW      1
#define ACW     2

byte xState = 0;
byte yState = 0;
byte zState = 0;
byte jawState = 0;
byte rotState = 0;

byte lastxState = 0;
byte lastyState = 0;
byte lastzState = 0;
byte lastjawState = 0;
byte lastrotState = 0;

bool move = false;

#define dead_pos 700
#define dead_neg -700


int map_x = 0;
int map_y = 0;
int map_z = 0;
int map_y2 = 0;
int val_jaw = 0;
int val_cw = 0;
int val_acw = 0;

//------------------------------------------------------------
uint8_t RxMACaddress[] = {0xC0, 0x49, 0xEF, 0xD0, 0xDB, 0xB8};
//------------------------------------------------------------
typedef struct TxStruct
{
  byte xData = STOP;
  byte yData = STOP;
  byte zData = STOP;
  bool jaw = CLOSE;
  byte rot = STOP;

} TxStruct;
TxStruct sentData;
//------------------------------------------------------------
typedef struct RxStruct
{
  int fsr;

} RxStruct;
RxStruct receivedData;

 esp_now_peer_info_t peerInfo;
//------------------------------------------------------------
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len)
{
  memcpy(&receivedData, incomingData, sizeof(receivedData));

  haptics();
  
}

//===================functions===================================================================

//state change for x link
void change_x() {
  if (map_x < dead_neg || map_x > dead_pos) {
    if (map_x > dead_pos) {
      xState = FWD;
    } else if (map_x < dead_neg) {
      xState = REV;
    }
  }
  else {
    xState = STOP;
  }
  return;
}




//state change for y link
void change_y() {
  if (map_y < dead_neg || map_y > dead_pos) {
    if (map_y > dead_pos) {
      yState = FWD;
    } else if (map_y < dead_neg) {
      yState = REV;
    }
  }
  else {
    yState = STOP;
  }
  return;
}

//state change for z link
void change_z() {
  if (map_z < dead_neg || map_z > dead_pos) {
    if (map_z > dead_pos) {
      zState = FWD;
    } else if (map_z < dead_neg) {
      zState = REV;
    }
  }
  else {
    zState = STOP;
  }
  return;
}


//state change for jaw
void change_jaw() {
  if (val_jaw == LOW ) {
    jawState = OPEN;
  }
  else {
    jawState = CLOSE;
  }
  //return;
}


//state change for ROTATION
void change_rot() {
  if (val_cw == LOW and val_acw == HIGH ) {
    rotState = CW;
  }
  else if (val_cw == HIGH and val_acw == LOW ) {
    rotState = ACW;
  }
  else {
    rotState = STOP;
  }
  //return;
}

//state for haptics
void haptics() {
  fsrVal = receivedData.fsr;

  // Map FSR value to motor speed
  //int motorSpeed = map(fsrVal, fsrThreshold, 4095, motorSpeedMin, motorSpeedMax);

  // Limit motor speed within the defined range
  //motorSpeed = constrain(motorSpeed, motorSpeedMin, motorSpeedMax);

  // Set motor speed
  motorA.motorGo(fsrVal);

}

//======================================================================================

// preparing and sending data
void cast() {
  if (xState != lastxState || yState != lastyState || zState != lastzState || jawState != lastjawState || rotState != lastrotState) {            //This makes the async xTX data flow
    sentData.xData = xState;
    sentData.yData = yState;
    sentData.zData = zState;
    sentData.jaw = jawState;
    sentData.rot = rotState;//Cast the changed state to the xTX callback

    //sending
    esp_err_t result = esp_now_send(RxMACaddress, (uint8_t *) &sentData, sizeof(sentData));


    // getting feedback message
    //feedback();
    if (result == ESP_OK) {
      char x_buf[20];
      char y_buf[20];
      char z_buf[20];
      char j_buf[20];
      char r_buf[20];
      String sx, sy, sz, sj, sr = "";

      if (xState == FWD) {
        sx = "X_REV";
      } else if (xState == REV) {
        sx = "X_FWD";
      } else if (xState == STOP) {
        sx = "X_STOP";
      }

      if (yState == FWD) {
        sy = "Y_REV";
      } else if (yState == REV) {
        sy = "Y_FWD";
      } else if (yState == STOP) {
        sy = "Y_STOP";
      }

      if (zState == FWD) {
        sz = "Z_REV";
      } else if (zState == REV) {
        sz = "Z_FWD";
      } else if (zState == STOP) {
        sz = "Z_STOP";
      }

      if (jawState == OPEN) {
        sj = "OPEN";
      } else if (jawState == CLOSE) {
        sj = "CLOSED";
      }

      if (rotState == CW) {
        sr = "CW";
      } else if (rotState == ACW) {
        sr = "ACW";
      } else if (rotState == STOP) {
        sr = "NO ROT";
      }

      sprintf(x_buf, "Sending %s", sx);
      sprintf(y_buf, "Sending %s", sy);
      sprintf(z_buf, "Sending %s", sz);
      sprintf(j_buf, "Sending %s", sj);
      sprintf(r_buf, "Sending %s", sr);

      Serial.println(x_buf);
      Serial.println(y_buf);
      Serial.println(z_buf);
      Serial.println(j_buf);
      Serial.println(r_buf);//Only prints when packet is sent
    } else {
      Serial.println("Failed to send data over ESP-NOW");
    }
  }

}

//======================================================================================


void setup()
{
  Serial.begin(115200);
  pinMode(open_pin, INPUT_PULLUP);
  pinMode(CW_BUTTON_PIN, INPUT_PULLUP);
  pinMode(ACW_BUTTON_PIN, INPUT_PULLUP);
  motorA.motorBrake();

  //----------------------------------------------------------
  WiFi.mode(WIFI_STA);
  //----------------------------------------------------------
  if (esp_now_init() != ESP_OK)
  {
    Serial.println("Error initializing ESP-NOW");
    return;
  }

  //----------------------------------------------------------
 // esp_now_peer_info_t peerInfo;
  memcpy(peerInfo.peer_addr, RxMACaddress, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  //----------------------------------------------------------
  if (esp_now_add_peer(&peerInfo) != ESP_OK)
  {
    Serial.println("Failed to add peer");
    return;
  }
  //----------------------------------------------------------
  esp_now_register_recv_cb(OnDataRecv);
}
//======================================================================================
void loop()
{
  //reading values
  val_jaw = digitalRead(open_pin);
  val_cw = digitalRead(CW_BUTTON_PIN);
  val_acw = digitalRead(ACW_BUTTON_PIN);
  int val_x = analogRead(x);
  int val_y = analogRead(y);
  int val_z = analogRead(z);
  //int val_y2 = analogRead(y2);

  //mapping
  map_x = map(val_x, 0, 4095, -1000, 1000);
  map_y = map(val_y, 0, 4095, -1000, 1000);
  map_z = map(val_z, 0, 4095, -1000, 1000);
  //map_y2 = map(val_y2, 0, 4095, -1000, 1000);
  //----------------------------------------------------------
  // observing state change
  change_x();
  change_y();
  change_z();
  change_jaw();
  change_rot();


  //casting, sending and getting feedback
  cast();
  //----------------------------------------------------------

  lastxState = xState;
  lastyState = yState;
  lastzState = zState;
  lastjawState = jawState;
  lastrotState = rotState;
  //----------------------------------------------------------
  //delay(1000);
}

Robot:

//--------------------------------------------
//ESP32 robot via ESP-NOW Protocol
//Transmit fsr - receive joyvals
//--------------------------------------------
#include "AccelStepper.h"
#include <ESP32MX1508.h>
#include <ESP32Servo.h>
#include <esp_now.h>
#include <WiFi.h>
#include <Wire.h>

//------------------------------------------------------------


// Yrotation driver pins
#define ROTA 12
#define ROTB 14
#define CH1 0
#define CH2 1
#define RES 12

MX1508 motorA(ROTA, ROTB, CH1, CH2, RES); //object for yrot motor


#define XSTEP   23
#define XDIR    22
#define YSTEP   33
#define YDIR    32
#define ZSTEP   26
#define ZDIR    25

#define DRIVER  1

AccelStepper stepperX(DRIVER, XSTEP, XDIR);
AccelStepper stepperY(DRIVER, YSTEP, YDIR);
AccelStepper stepperZ(DRIVER, ZSTEP, ZDIR);

int servoPin = 18;
Servo myservo;

//int fsrValue = 0;

//STATES
#define FWD     2
#define REV     1
#define STOP    0
#define OPEN    0
#define CLOSE   1
#define CW      1
#define ACW     2

//------------------------------------------------------------
uint8_t RxMACaddress[] = {0xEC, 0x62, 0x60, 0x9A, 0x62, 0x60};
//------------------------------------------------------------
typedef struct TxStruct
{
  int fsr;

} TxStruct;
TxStruct sentData;
//------------------------------------------------------------
typedef struct RxStruct
{
  byte xData = STOP;
  byte yData = STOP;
  byte zData = STOP;
  bool jaw = CLOSE;
  byte rot = STOP;
} RxStruct;
RxStruct receivedData;


byte lastxState = 0;
byte lastyState = 0;
byte lastzState = 0;
byte lastjawState = 0;
byte lastrotState = 0;

//byte lastfsrValue = 0;


bool xmove = false;
bool ymove = false;
bool zmove = false;

esp_now_peer_info_t peerInfo;
//------------------------------------------------------------
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len)
{
  memcpy(&receivedData, incomingData, sizeof(receivedData));

  if (lastxState != receivedData.xData || lastyState != receivedData.yData || lastzState != receivedData.zData || lastjawState != receivedData.jaw || lastrotState != receivedData.rot) {

    // moving x
    if (receivedData.xData == FWD) {
      stepperX.setSpeed(500);
      xmove = true;
    } else if (receivedData.xData == REV) {
      stepperX.setSpeed(-500);
      xmove = true;
    } else if (receivedData.xData == STOP) {
      xmove = false;
      stepperX.stop();
    }

    // moving y
    if (receivedData.yData == FWD) {
      stepperY.setSpeed(500);
      ymove = true;
    } else if (receivedData.yData == REV) {
      stepperY.setSpeed(-500);
      ymove = true;
    } else if (receivedData.yData == STOP) {
      ymove = false;
      stepperY.stop();
    }


    //Moving Z
    if (receivedData.zData == FWD) {
      stepperZ.setSpeed(500);
      zmove = true;
    } else if (receivedData.zData == REV) {
      stepperZ.setSpeed(-500);
      zmove = true;
    } else if (receivedData.zData == STOP) {
      zmove = false;
      stepperZ.stop();
    }

    // jaw
    if (receivedData.jaw == OPEN) {
      myservo.write(0);
     
    } else {
      myservo.write(180);
     
    }

    // rot
    if (receivedData.rot == CW) {
      motorA.motorGo(4095);
    } else if (receivedData.rot == ACW) {
      motorA.motorRev(4095);
    }
    else if (receivedData.rot == STOP) {
      motorA.motorBrake();
    }

    lastxState = receivedData.xData;
    lastyState = receivedData.yData;
    lastzState = receivedData.zData;
    lastjawState = receivedData.jaw;
    lastrotState = receivedData.rot;

  }

  //  lastxState = receivedData.xData;
  //  lastyState = receivedData.yData;
  //  lastzState = receivedData.zData;
  //  lastjawState = receivedData.jaw;
  //  lastrotState = receivedData.rot;
}

//======================================================================================
void setup()
{
  Serial.begin(115200);
  myservo.attach(servoPin, 500, 2400);
  pinMode(34, INPUT);

  stepperX.setMaxSpeed(1000);
  stepperX.setAcceleration(30);
  stepperX.setSpeed(50);
  stepperX.stop();

  stepperY.setMaxSpeed(1000);
  stepperY.setAcceleration(30);
  stepperY.setSpeed(50);
  stepperY.stop();

  stepperZ.setMaxSpeed(1000);
  stepperZ.setAcceleration(30);
  stepperZ.setSpeed(50);
  stepperZ.stop();

  //----------------------------------------------------------
  WiFi.mode(WIFI_STA);
  if (esp_now_init() != ESP_OK)
  {
    Serial.println("Error initializing ESP-NOW");
    return;
  }

  //----------------------------------------------------------

  memcpy(peerInfo.peer_addr, RxMACaddress, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  //----------------------------------------------------------
  if (esp_now_add_peer(&peerInfo) != ESP_OK)
  {
    Serial.println("Failed to add peer");
    return;
  }
  //----------------------------------------------------------
  esp_now_register_recv_cb(OnDataRecv);
}
//======================================================================================
void loop()
{
  // put your main code here, to run repeatedly:
  if (xmove) {
    stepperX.runSpeed();
  }

  if (ymove) {
    stepperY.runSpeed();
  }

  if (zmove) {
    stepperZ.runSpeed();
  }

  //reading fsr
  sentData.fsr = analogRead(34);

  esp_err_t result = esp_now_send(RxMACaddress, (uint8_t *) &sentData, sizeof(sentData));
  //----------------------------------------------------------
  if (result == ESP_OK) Serial.println("Sent with success");
  else Serial.println("Error sending the data");
}

I've tested the servo individually on ESPNOW and it works but in the final code it doesn't work.

2

Please explain what "doesn't work" means. What do you expect to happen, and what happens instead?

A very large fraction of problems with motor and servo projects are due to inadequate motor and servo power supplies. Therefore, please post a schematic/wiring diagram and links to all the major components.

A photo of a hand drawn schematic is best, with pins and connections clearly labeled.

3

doesn't work means doesn't work, nothing happens, all I can want it to run which it's not.

the data is being transmitted but the motor is not moving.

WhatsApp Image 2023-05-26 at 8.53.45 PM
WhatsApp Image 2023-05-26 at 8.53.45 PM780×1040 26.6 KB

I'm powering esp32 with usb attached to laptop and servo with external 5V with ground being common for both esp and servo.

4

But no signal goes to the servo. What have you done to debug that?

5

I serial printed the bool values received at the robot site and they are clearly visible. but it doesn't affect motor

6

Servos signals are not boolean, they are PWM values. Use your scope, multimeter or logic probe to check for PWM output on pin 18.

7

Please see the complete code. I've used bool values for communication, later translated them to meaningful commands like pwm etc

8

No, YOU need to check for signal output on pin 18.

9

add serial printing that prints to the serial monitor what values where assigned to your sendstruct
right before you call

esp_err_t result = esp_now_send(RxMACaddress, (uint8_t *) &sentData, sizeof(sentData));

add serial printing to the robots code inside the function

void OnDataRecv(

right below the receiving

memcpy(&receivedData, incomingData, sizeof(receivedData));

and compare sended data with received data

best regards Stefan

10

Okay, let me check

11

done that already. It's fine.

13

Interesting, I used DMM, and found pin 18 to be constantly at ~3.3V.
Note: when I used ESPNOW sketches for the servo only (without other things like steppers etc in the code) pin 18 shows pwm signal.

14

Put in Serial.print() statements to verify that the expected servo command functions are actually being called.

15

Tried it and they are being called.

16

Perhaps you have a library or hardware conflict. But first, use some other method to verify the presence or absence of the expected PWM signal on pin 18. Scope or logic analyzer are better than a DMM.

17

Can your DMM measure frequency?

add a delay(5000) directly behind

do a reset of your ESP32 in parallel to measuring the voltage.

measure one time with sevo connected one time with servo disconnected

18

I measured it while using a sketch having only one servo in espnow. It clearly shows the pulse.

19

Then I would suspect a library or hardware conflict. Check for the possibility that pin 18 servo function is overridden by some other peripheral, in the problematic code.

You can try reassigning pin functions.

20

how can I check that? in the code it's only declared as servoPin.

21

should I change the pin for servo?