ESP32 and servo motor

Projects Programming
1

I have two separate breadboards, each with an ESP32 module. One acts as the transmitter, and the other as the receiver. The receiver is connected to a servo motor, along with a few other components that are not relevant for now.

Here’s what I want the servo motor to do:

  • When the RSSI (signal strength) between the two ESP32 modules drops below a certain threshold (e.g., -50 dBm), I want the servo motor to rotate 90 degrees clockwise (from 0° to 90°) and stop there.

  • When the RSSI rises above a different threshold (e.g., -47 dBm), I want the servo motor to rotate 90 degrees counterclockwise (from 90° back to 0°) and stop.

the initial postion of the servo motor is 0°

reciver code:

#include <ESP32Servo.h>
#include <esp_now.h>
#include <WiFi.h>
#include <esp_wifi.h>

#define SERVO_PIN 26    
#define RED_LED_PIN 32
#define GREEN_LED_PIN 25 
#define BUZZER_PIN 27   

#define SAMPLES 30
#define DEAD_BAND_SPEED 0.05
#define DEAD_BAND_DISTANCE 0.05

#define BASE_ACTIVATE_RSSI -50
#define BASE_DEACTIVATE_RSSI -47

Servo servo;
bool systemActive = false;
bool overrideActive = false;
bool prevSystemState = false;
float rssiSamples[SAMPLES];
float kalmanFilteredRSSI = 0;
int sampleIndex = 0;
volatile int latestRSSI = 0;

float kalmanRSSI = 0;
float kalmanP = 1;
const float Q = 0.5;
const float R = 2.0;

static float prevRSSI = 0;
static unsigned long prevTime = 0;

bool buzzerActive = false;
unsigned long lastBuzzerTime = 0;
int buzzerFreq = 1000;
bool increasing = true;

struct PacketData {
  byte type; // 0 = RSSI, 1 = BUTTON
};
PacketData data;

float kalmanUpdate(float measurement) {
    kalmanP = kalmanP + Q;
    float K = kalmanP / (kalmanP + R);
    kalmanRSSI = kalmanRSSI + K * (measurement - kalmanRSSI);
    kalmanP = (1 - K) * kalmanP;
    return kalmanRSSI;
}

void promiscuousRxCB(void *buf, wifi_promiscuous_pkt_type_t type) {
    if (type == WIFI_PKT_MGMT) {
        wifi_promiscuous_pkt_t *pkt = (wifi_promiscuous_pkt_t *)buf;
        latestRSSI = pkt->rx_ctrl.rssi;
    }
}

void updateSiren() {
    if (buzzerActive) {
        unsigned long currentMillis = millis();
        if (currentMillis - lastBuzzerTime >= 50) {
            lastBuzzerTime = currentMillis;
            buzzerFreq += increasing ? 100 : -100;
            if (buzzerFreq >= 3000) increasing = false;
            if (buzzerFreq <= 1000) increasing = true;
            tone(BUZZER_PIN, buzzerFreq);
        }
    } else {
        noTone(BUZZER_PIN);
    }
}

void activateSystem() {
    Serial.println("System Activated");
    systemActive = true;
    servo.attach(SERVO_PIN);
    servo.write(180);  
    delay(300);
    digitalWrite(RED_LED_PIN, HIGH);
    digitalWrite(GREEN_LED_PIN, LOW);
    buzzerActive = true;
}

void deactivateSystem() {
    Serial.println("System Deactivated");
    systemActive = false;
    servo.write(0);
    delay(300);
    servo.detach();
    digitalWrite(RED_LED_PIN, LOW);
    digitalWrite(GREEN_LED_PIN, HIGH);
    buzzerActive = false;
}

void OnDataRecv(const esp_now_recv_info_t *info, const uint8_t *incomingData, int len) {
    memcpy(&data, incomingData, sizeof(data));

    if (data.type == 1) { // Override button
        overrideActive = !overrideActive;
        Serial.print("Override Mode: ");
        Serial.println(overrideActive ? "ON" : "OFF");

        if (overrideActive) {
            prevSystemState = systemActive;
            if (!systemActive) activateSystem();
        } else {
            if (kalmanFilteredRSSI < BASE_ACTIVATE_RSSI) {
                if (!systemActive) activateSystem();
            } else {
                if (prevSystemState && !systemActive) activateSystem();
                else if (!prevSystemState && systemActive) deactivateSystem();
            }
        }
    }

    if (data.type == 0) { // RSSI packet
        rssiSamples[sampleIndex] = latestRSSI;
        sampleIndex++;
        if (sampleIndex >= SAMPLES) sampleIndex = 0;

        float avgRSSI = 0;
        for (int i = 0; i < SAMPLES; i++) avgRSSI += rssiSamples[i];
        avgRSSI /= SAMPLES;

        kalmanFilteredRSSI = kalmanUpdate(avgRSSI);

        if (abs(kalmanFilteredRSSI - prevRSSI) < DEAD_BAND_DISTANCE) {
            kalmanFilteredRSSI = prevRSSI;
        }

        unsigned long now = millis();
        float deltaT = (now - prevTime) / 1000.0;
        float speed = deltaT > 0 ? (kalmanFilteredRSSI - prevRSSI) / deltaT : 0;
        prevTime = now;
        prevRSSI = kalmanFilteredRSSI;

        if (abs(speed) < DEAD_BAND_SPEED) {
            speed = 0.0;
        }

        Serial.print("RSSI:");
        Serial.println(kalmanFilteredRSSI);

        if (!overrideActive) {
            if (kalmanFilteredRSSI < BASE_ACTIVATE_RSSI && !systemActive) {
                activateSystem();
            } else if (kalmanFilteredRSSI > BASE_DEACTIVATE_RSSI && systemActive) {
                deactivateSystem();
            }
        }
    }
}

void setup() {
    Serial.begin(115200);
    WiFi.mode(WIFI_STA);
    WiFi.disconnect();
    delay(100);

    if (esp_now_init() != ESP_OK) {
        Serial.println("ESP-NOW Init Failed");
        return;
    }
    esp_now_register_recv_cb(OnDataRecv);

    pinMode(RED_LED_PIN, OUTPUT);
    pinMode(GREEN_LED_PIN, OUTPUT);
    pinMode(BUZZER_PIN, OUTPUT);
    digitalWrite(RED_LED_PIN, LOW);
    digitalWrite(GREEN_LED_PIN, HIGH);
    noTone(BUZZER_PIN);

    servo.attach(SERVO_PIN);
    delay(200);
    servo.write(0);
    delay(500);
    servo.detach();

    esp_wifi_set_promiscuous(true);
    esp_wifi_set_promiscuous_rx_cb(&promiscuousRxCB);

    Serial.println("System ready with RSSI + Override button");
}

void loop() {
    updateSiren();
}

transmitter code:

#include <esp_now.h>
#include <WiFi.h>
#include <esp_wifi.h>

uint8_t receiverMacAddress[] = {0x20, 0x43, 0xA8, 0x63, 0x35, 0xA8};  // Update to receiver's MAC

#define LED_PIN 22  // LED connected to pin 22

struct PacketData {
  byte switch1Value;
  int rssiValue;
};
PacketData data;

bool lastButtonState = false;
bool overrideActive = false;

void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("Send Status: ");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Success" : "Failure");
}

void setup() {
  Serial.begin(115200);
  WiFi.mode(WIFI_STA);

  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW Init Failed");
    return;
  }
  Serial.println("ESP-NOW Initialized");

  esp_now_register_send_cb(OnDataSent);

  esp_now_peer_info_t peerInfo;
  memcpy(peerInfo.peer_addr, receiverMacAddress, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;

  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add peer");
    return;
  }
  Serial.println("Peer added successfully");

  pinMode(15, INPUT_PULLUP);
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);
}

void loop() {
  wifi_ap_record_t apinfo;
  if (esp_wifi_sta_get_ap_info(&apinfo) == 0) {
    data.rssiValue = apinfo.rssi;
  } else {
    data.rssiValue = -100;
  }

  bool buttonState = !digitalRead(15);
  if (buttonState && !lastButtonState) {
    overrideActive = !overrideActive;
    data.switch1Value = 1;
    digitalWrite(LED_PIN, overrideActive ? HIGH : LOW);
    Serial.print("Override State: ");
    Serial.println(overrideActive ? "ON" : "OFF");
  } else {
    data.switch1Value = 0;
  }
  lastButtonState = buttonState;

  esp_err_t result = esp_now_send(receiverMacAddress, (uint8_t *)&data, sizeof(data));
  if (result == ESP_OK) {
    Serial.print("Packet Sent | RSSI: ");
    Serial.print(data.rssiValue);
    Serial.print(" dBm | Button: ");
    Serial.println(data.switch1Value);
  } else {
    Serial.println("Send Error");
  }

  delay(50);
}
2

So… what's stopping you?

You didn't say I don't think where the servo is, at the transmitter or the receiver.

a7

3

It's on the receiver.
For some reason it just doesn't work. I tried doing something like this:

void activateSystem() {
    Serial.println("System Activated");
    systemActive = true;
    servo.attach(SERVO_PIN);
    servo.write(90);  
    delay(300);
    servo.detach();
    digitalWrite(RED_LED_PIN, HIGH);
    digitalWrite(GREEN_LED_PIN, LOW);
    buzzerActive = true;
}

void deactivateSystem() {
    Serial.println("System Deactivated");
    systemActive = false;
    servo.attach(SERVO_PIN);
    servo.write(0);  
    delay(300);
    servo.detach();
    digitalWrite(RED_LED_PIN, LOW);
    digitalWrite(GREEN_LED_PIN, HIGH);
    buzzerActive = false;
}

But what actually happens is that as soon as I go out of range, the motor moves about 20 degrees and stops, and when I return to range, it moves almost 180 degrees and stops.

4

Did you ever test your servos separately with some example?
They are not "continuous rotation" servos?

5

this is what i got:
https://a.aliexpress.com/_onJFyrH

6

If you selected that 360deg option, you cant's control the position of your servo.

If you write 90, it stops servo. Any number below 90 makes it move one direction and above another direction. Speed depends on difference to 90, for example 0 is max speed, 89 very slow speed. You can have some pseudo-position control by controlling speed and time.

7

Is it printing the correct stuff and the LEDs are also correctly showing?

If you have a 360 degree servo, activateSystem() would do nothing and deactiaveSystem() would spin for 300 millisconds, a plausible time for scooting 180 degrees.

While you can do some positioning using time, it will be inaccurate and eventually not be stopping at all where you wanted.

You could get real srevos, in theis case the ones called 180. Obvsly Ihope you know that would limit travel of the servo arm to a nominal 180 degrees.

Or you could add some kind of encoder to the servo you have and program a feedback loop thus accomplishing position control. Fun of a kind that mightn't be so much. Fun.

HTH

a7

8

so Servo motor of 180 deg should work?

9

Yes.

But it might make sense to get out the servo sweep example found in the IDE and tinker with it and learn if you have a continuous rotation servo in front of you, or some other issue we haven't yet addressed.

a7

10

Normal servo turns 180deg and has position control, the one you have (360) is technically not servo at all.
But it can be useful for other purposes, you have small gear motor with direction and speed control which doesn't need separate driver.

11

Okay, thank you very much!
Actually, after a lot of playing around and calibrating, I managed to achieve this, but of course only for now until I get the 180 motor.

void activateSystem() {
    Serial.println(" System Activated!");
    systemActive = true;
    servo.attach(SERVO_PIN);

    servo.writeMicroseconds(1700);
    delay(530);
    servo.writeMicroseconds(1500);
    delay(100);
    servo.detach();

    digitalWrite(RED_LED_PIN, HIGH);
    digitalWrite(GREEN_LED_PIN, LOW);
    buzzerActive = true;
}

void deactivateSystem() {
    Serial.println(" System Deactivated!");
    systemActive = false;
    servo.attach(SERVO_PIN);

    servo.writeMicroseconds(1300);
    delay(350);
    servo.writeMicroseconds(1500);
    delay(100);
    servo.detach();

    digitalWrite(RED_LED_PIN, LOW);
    digitalWrite(GREEN_LED_PIN, HIGH);
    buzzerActive = false;
}
12

Hey good work, I see what you did there.

When you have a "real" servo, it won't be necessary to detach it.

In fact, you probably don't need to now. Just attach it once in setup(), lose the detach statements.

Writing 1500 us or 90 degrees should stop the servo. That's a problem with the 360, 1500 might not exactly stop it, so you might have to experiment.

But life is short. Later when you are writing to the new servo, you won't need to attach and detach it. The servo will go to the angle you said, and then hold there.

Holding might be important if the servo is doing much more than waving a tiny little flag.

a7

13

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.