Programski jezici → Arduino i ESP32 → Rampa i radar (ESP32)

ESP32: servo motor i ultrazvučni senzor – automatska rampa i radar

Spajamo dve stvari koje već znamo: ultrazvučni senzor HC-SR04 meri rastojanje, a servo motor se okreće na zadati ugao. Zajedno prave automatsku rampu (podiže se kada joj priđe automobil) i radar koji „pretražuje“ okolinu i crta prepreke na ekranu računara.

Povezivanje

Program 1: automatska rampa

Kada je prepreka bliže od 20 cm, servo se okreće na 90° (rampa gore); kada se prepreka udalji – vraća se na 0°.

// ESP32 – automatska rampa: servo se okreće kada je prepreka bliže od granice
#include <ESP32Servo.h>

const int TRIG_PIN = 5;
const int ECHO_PIN = 18;        // ako se HC-SR04 napaja sa 5 V – Echo preko naponskog delitelja!
const int SERVO_PIN = 13;
const float DISTANCE_THRESHOLD = 20.0;   // granica u cm

Servo servo;
long duration_us;
float distance_cm;

void setup() {
  Serial.begin(115200);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  servo.setPeriodHertz(50);
  servo.attach(SERVO_PIN, 500, 2400);
  servo.write(0);               // rampa spuštena
}

void loop() {
  // Slanje impulsa od 10 µs na TRIG
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // Merenje vremena povratka (najduže 30 ms – oko 5 m)
  duration_us = pulseIn(ECHO_PIN, HIGH, 30000);

  // Izračunavanje rastojanja: zvuk prelazi 0,034 cm/µs, put je tamo i nazad
  distance_cm = duration_us * 0.034 / 2;

  Serial.print("Rastojanje: ");
  Serial.print(distance_cm);
  Serial.println(" cm");

  // Upravljanje servo motorom (0 cm znači da odjek nije stigao)
  if (distance_cm > 0 && distance_cm < DISTANCE_THRESHOLD) {
    servo.write(90);            // podigni rampu
  } else {
    servo.write(0);             // spusti rampu
  }
  delay(500);
}

• pulseIn(ECHO_PIN, HIGH, 30000) čeka odjek najviše 30 ms. Ako odjek ne stigne (prepreka predaleko), vraća 0 – zato u uslovu stoji i distance_cm > 0.
• Zvuk prelazi oko 0,034 cm za jednu mikrosekundu. Talas ide do prepreke i nazad, pa delimo sa 2.

Program 2: servo „maše“ kada je prepreka blizu

Umesto jednog položaja, servo kada primeti prepreku pređe ceo put od 0° do 180° i nazad. Merenje je izdvojeno u funkciju izmeriRastojanje(), pa je loop() kraći i čitljiviji.

// ESP32 – kada je prepreka blizu, servo „maše“ od 0° do 180° i nazad
#include <ESP32Servo.h>

const int TRIG_PIN = 5;
const int ECHO_PIN = 18;
const int SERVO_PIN = 13;
const float DISTANCE_THRESHOLD = 20.0;

Servo servo;

float izmeriRastojanje() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  long trajanje = pulseIn(ECHO_PIN, HIGH, 30000);
  return trajanje * 0.034 / 2;           // u cm (0 ako nema odjeka)
}

void setup() {
  Serial.begin(115200);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  servo.setPeriodHertz(50);
  servo.attach(SERVO_PIN, 500, 2400);
}

void loop() {
  float distance_cm = izmeriRastojanje();
  Serial.print("Rastojanje: ");
  Serial.print(distance_cm);
  Serial.println(" cm");

  if (distance_cm > 0 && distance_cm < DISTANCE_THRESHOLD) {
    // Sweep 0 -> 180
    for (int pos = 0; pos <= 180; pos++) {
      servo.write(pos);
      delay(10);
    }
    // Sweep 180 -> 0
    for (int pos = 180; pos >= 0; pos--) {
      servo.write(pos);
      delay(10);
    }
  }
  delay(100);
}

Program 3: radar

Za radar senzor pričvrstimo na krak servo motora (lepljivom trakom ili držačem). Servo se okreće stepen po stepen, a na svakom uglu meri se rastojanje. ESP32 šalje redove oblika ugao,rastojanje, npr. 90,34.

// ESP32 – radar: senzor na servo motoru skenira od 15° do 165°
// Za svaki ugao šalje red „ugao,rastojanje“ – pogodno za prikaz u Processing-u ili Serial Plotter-u
#include <ESP32Servo.h>

const int TRIG_PIN = 5;
const int ECHO_PIN = 18;
const int SERVO_PIN = 13;

Servo servo;

int izmeriCm() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  long trajanje = pulseIn(ECHO_PIN, HIGH, 30000);
  return (int)(trajanje * 0.034 / 2);
}

void skeniraj(int ugao) {
  servo.write(ugao);
  delay(30);                    // servo mora da stigne pre merenja
  int cm = izmeriCm();
  Serial.print(ugao);
  Serial.print(",");
  Serial.println(cm);
}

void setup() {
  Serial.begin(115200);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  servo.setPeriodHertz(50);
  servo.attach(SERVO_PIN, 500, 2400);
}

void loop() {
  for (int u = 15; u <= 165; u++) {
    skeniraj(u);
  }
  for (int u = 165; u >= 15; u--) {
    skeniraj(u);
  }
}

Već u Serial Plotter-u (Tools → Serial Plotter) vidi se kriva rastojanja. Lepši prikaz dobijamo u Processing-u: program crta polukrug kao na ekranu radara, a prepreke kao crvene tačke.

// Processing – prikaz radara (podaci „ugao,rastojanje“ sa ESP32)
import processing.serial.*;

Serial port;
float[] rastojanja = new float[181];   // poslednje rastojanje za svaki ugao
int ugao = 0;
final float MAX_CM = 100;              // domet prikaza

void setup() {
  size(800, 450);
  println(Serial.list());              // spisak portova – izaberi pravi indeks
  port = new Serial(this, Serial.list()[0], 115200);
  port.bufferUntil('\n');
}

void draw() {
  background(0, 30, 0);
  translate(width / 2, height - 20);
  stroke(0, 160, 0);
  noFill();
  for (int r = 1; r <= 4; r++) {       // kružnice na 25, 50, 75 i 100 cm
    arc(0, 0, r * 190, r * 190, PI, TWO_PI);
  }
  // tačke prepreka
  noStroke();
  fill(255, 60, 60);
  for (int a = 0; a <= 180; a++) {
    float cm = rastojanja[a];
    if (cm > 0 && cm < MAX_CM) {
      float r = map(cm, 0, MAX_CM, 0, 380);
      ellipse(r * cos(radians(a)), -r * sin(radians(a)), 6, 6);
    }
  }
  // zrak koji pokazuje trenutni ugao
  stroke(0, 255, 0);
  line(0, 0, 380 * cos(radians(ugao)), -380 * sin(radians(ugao)));
}

void serialEvent(Serial p) {
  String red = trim(p.readStringUntil('\n'));
  if (red == null) return;
  String[] delovi = split(red, ',');
  if (delovi.length == 2) {
    int a = int(delovi[0]);
    float cm = float(delovi[1]);
    if (a >= 0 && a <= 180) {
      ugao = a;
      rastojanja[a] = cm;
    }
  }
}

• Pre pokretanja zatvori Serial Monitor u Arduino IDE-u – port može da koristi samo jedan program.
• Ako Processing ne prima podatke, pogledaj spisak portova koji ispiše println(Serial.list()) i promeni indeks u Serial.list()[0].
• Ugao se pretvara u tačku pomoću cos i sin – isto kao u lekcijama o kretanju po krugu u Processing-u.

Zadaci za vežbu

1. Dodaj LED diodu koja svetli crveno dok je rampa podignuta.
2. Neka rampa ostane gore još 3 sekunde pošto prepreka ode (koristi millis()).
3. Posle jednog prolaza radara ispiši u kom smeru je najbliža prepreka i okreni senzor ka njoj.
4. U Processing programu ispiši rastojanje do najbliže tačke i ugao pod kojim je.

// Zadatak – posle jednog prolaza ispiši u kom smeru je najbliža prepreka
#include <ESP32Servo.h>

const int TRIG_PIN = 5;
const int ECHO_PIN = 18;
const int SERVO_PIN = 13;

Servo servo;

int izmeriCm() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  long trajanje = pulseIn(ECHO_PIN, HIGH, 30000);
  return (int)(trajanje * 0.034 / 2);
}

void setup() {
  Serial.begin(115200);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  servo.setPeriodHertz(50);
  servo.attach(SERVO_PIN, 500, 2400);
}

void loop() {
  int najmanje = 10000;         // veće od svakog stvarnog rastojanja
  int najboljiUgao = -1;
  for (int u = 0; u <= 180; u += 5) {
    servo.write(u);
    delay(80);
    int cm = izmeriCm();
    if (cm > 0 && cm < najmanje) {   // 0 = nema odjeka, ne računamo
      najmanje = cm;
      najboljiUgao = u;
    }
  }
  if (najboljiUgao >= 0) {
    Serial.print("Najbliza prepreka: ");
    Serial.print(najmanje);
    Serial.print(" cm, ugao ");
    Serial.println(najboljiUgao);
    servo.write(najboljiUgao);  // senzor „gleda“ ka prepreci
  } else {
    Serial.println("Nema prepreka u dometu.");
  }
  delay(3000);
}

Proveri znanje