My obstacle avoiding robot using esp 32

Projects General Guidance
1

My robot bumps into wall and then stops and scan
#include <ESP32Servo.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ST7735.h>
#include <SPI.h>
#include <AnimatedGIF.h>
#include <SD.h>

// Motor control pins
#define ENA 32
#define ENB 33
#define IN1 17
#define IN2 16
#define IN3 0
#define IN4 15
#define IR_BOTTOM 34

// Ultrasonic sensor pins
#define TRIG_PIN_FRONT 13
#define ECHO_PIN_FRONT 12
#define TRIG_PIN_LEFT 14
#define ECHO_PIN_LEFT 27
#define TRIG_PIN_RIGHT 26
#define ECHO_PIN_RIGHT 25
#define MAX_DISTANCE 20

// Display pins
#define TFT_CS 5
#define TFT_RST 4
#define TFT_DC 3

// SD Card pins
#define SD_CS 2

Adafruit_ST7735 tft = Adafruit_ST7735(TFT_CS, TFT_DC, TFT_RST);
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
AnimatedGIF gif;

unsigned long previousMillis = 0;
const long interval = 60000;

// GIF file management functions
File file;
void * GIFOpenFile(const char *fname, int32_t *pSize) {
file = SD.open(fname);
if (file) {
*pSize = file.size();
return (void *)&file;
}
return NULL;
}

void GIFCloseFile(void *pHandle) {
File *f = (File *)pHandle;
if (f != NULL)
f->close();
}

int32_t GIFReadFile(GIFFILE *pFile, uint8_t *pBuf, int32_t iLen) {
int32_t iBytesRead;
iBytesRead = iLen;
File *f = (File *)pFile->fHandle;
if ((pFile->iSize - pFile->iPos) < iLen)
iBytesRead = pFile->iSize - pFile->iPos - 1;
if (iBytesRead <= 0)
return 0;
iBytesRead = f->read(pBuf, iBytesRead);
pFile->iPos += iBytesRead;
return iBytesRead;
}

int32_t GIFSeekFile(GIFFILE *pFile, int32_t iPosition) {
File *f = (File *)pFile->fHandle;
f->seek(iPosition);
pFile->iPos = (int32_t)f->position();
return pFile->iPos;
}

// GIFDraw function
void GIFDraw(GIFDRAW *pDraw) {
uint16_t usTemp;
uint8_t *s;
uint16_t *usPalette, usPixel;

s = pDraw->pPixels;
usPalette = pDraw->pPalette;
int y = pDraw->iY + pDraw->y;

for (int x = 0; x < pDraw->iWidth; x++) {
usPixel = usPalette[*s++];
if (usPixel != pDraw->ucTransparent) {
tft.drawPixel(x + pDraw->iX, y, tft.color565(usPixel >> 11, (usPixel >> 5) & 0x3F, usPixel & 0x1F));
}
}
}

void setup() {
// Initialize motor control pins
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(IR_BOTTOM, INPUT);

// Initialize ultrasonic sensors
pinMode(TRIG_PIN_FRONT, OUTPUT);
pinMode(ECHO_PIN_FRONT, INPUT);
pinMode(TRIG_PIN_LEFT, OUTPUT);
pinMode(ECHO_PIN_LEFT, INPUT);
pinMode(TRIG_PIN_RIGHT, OUTPUT);
pinMode(ECHO_PIN_RIGHT, INPUT);

// Initialize PCA9685 PWM driver
pwm.begin();
pwm.setPWMFreq(60);

// Initialize the TFT display
tft.initR(INITR_144GREENTAB);
tft.fillScreen(ST7735_BLACK);

// Initialize the SD card
if (!SD.begin(SD_CS)) {
return;
}

// Initialize GIF
gif.begin(LITTLE_ENDIAN_PIXELS);

// Move servos to initial position
pwm.setPWM(0, 0, 409);
pwm.setPWM(1, 0, 409);
pwm.setPWM(2, 0, 409);
}

void loop() {
// Check bottom IR sensor
if (digitalRead(IR_BOTTOM) == HIGH) {
stopRobot();
displayGIF("/pickup.gif");
} else {
// Move forward by default
moveForward();
displayGIF("/forward.gif");

// Check for obstacles
int distanceFront = getAverageDistance(TRIG_PIN_FRONT, ECHO_PIN_FRONT);
int distanceLeft = getAverageDistance(TRIG_PIN_LEFT, ECHO_PIN_LEFT);
int distanceRight = getAverageDistance(TRIG_PIN_RIGHT, ECHO_PIN_RIGHT);

if (distanceFront < MAX_DISTANCE || distanceLeft < MAX_DISTANCE || distanceRight < MAX_DISTANCE) {
  stopRobot();
  displayGIF("/obstacle.gif");
  scanWithServo();
  avoidObstacle(distanceFront, distanceLeft, distanceRight);
  centerServo();
}

}

// Move the servos every 2 minutes
unsigned long currentMillis = millis();
if (currentMillis - previousMillis >= interval) {
previousMillis = currentMillis;
moveServos();
}

delay(50);
}

void moveForward() {
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
analogWrite(ENA, 180);
analogWrite(ENB, 180);
}

void stopRobot() {
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
}

// Function to take multiple distance readings and average them
long getAverageDistance(int trigPin, int echoPin) {
long totalDistance = 0;
int validReadings = 0;
int readings = 5; // Number of readings to average
const int minValidDistance = 2; // Minimum valid distance in cm
const int maxValidDistance = 400; // Maximum valid distance in cm

for (int i = 0; i < readings; i++) {
long distance = getDistance(trigPin, echoPin);
if (distance >= minValidDistance && distance <= maxValidDistance) {
totalDistance += distance;
validReadings++;
}
delay(10); // Short delay between readings
}

if (validReadings > 0) {
return totalDistance / validReadings;
} else {
return maxValidDistance; // Return max distance if no valid readings
}
}

long getDistance(int trigPin, int echoPin) {
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
long duration = pulseIn(echoPin, HIGH, 30000); // Adding timeout to pulseIn
long distance = (duration / 2) / 29.1;
return distance;
}

void scanWithServo() {
for (int pos = 204; pos <= 614; pos += 40) { // Increase step size to reduce delay
pwm.setPWM(0, 0, pos);
delay(20); // Reduce delay
}
for (int pos = 614; pos >= 204; pos -= 40) {
pwm.setPWM(0, 0, pos);
delay(20);
}
centerServo();
}

void centerServo() {
pwm.setPWM(0, 0, 409);
}

void moveServos() {
pwm.setPWM(1, 0, 429);
pwm.setPWM(2, 0, 429);
delay(500);

pwm.setPWM(1, 0, 409);
pwm.setPWM(2, 0, 409);
}

void avoidObstacle(int distanceFront, int distanceLeft, int distanceRight) {
if (distanceFront < MAX_DISTANCE) {
if (distanceLeft < MAX_DISTANCE && distanceRight < MAX_DISTANCE) {
reverseAndScan();
} else if (distanceLeft < MAX_DISTANCE) {
turnRight();
} else if (distanceRight < MAX_DISTANCE) {
turnLeft();
} else {
turnLeft();
}
} else if (distanceLeft < MAX_DISTANCE) {
stopScan();
turnRight();
} else if (distanceRight < MAX_DISTANCE) {
stopScan();
turnLeft();
} else {
moveForward();
}
}

void stopScan() {
pwm.setPWM(0, 0, 409);
}

void reverseAndScan() {
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
delay(500);

scanWithServo();
if (getAverageDistance(TRIG_PIN_LEFT, ECHO_PIN_LEFT) > MAX_DISTANCE) {
turnLeft();
} else if (getAverageDistance(TRIG_PIN_RIGHT, ECHO_PIN_RIGHT) > MAX_DISTANCE) {
turnRight();
} else {
turnLeft();
}
}

void turnLeft() {
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
delay(600);
}

void turnRight() {
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
delay(600);
}

void displayGIF(const char *filename) {
tft.fillScreen(ST7735_BLACK);
gif.open(filename, GIFOpenFile, GIFCloseFile, GIFReadFile, GIFSeekFile, GIFDraw);

unsigned long frameDelay = 100; // Adjust this value to change the GIF frame rate
unsigned long previousFrameMillis = 0;

while (gif.playFrame(true, NULL)) {
unsigned long currentMillis = millis();
if (currentMillis - previousFrameMillis >= frameDelay) {
previousFrameMillis = currentMillis;
gif.playFrame(true, NULL); // Play the next frame
}
}
}
2

AND ?? :astonished:

3

In the IDE, click on Edit, then Copy for Forum and paste your code here.
You can't do a plain copy and paste

4

Bad wiring can be a reason.

5

Many places in your code make the motors run blind for whatever the "delay()" receives. You need to make those delays smaller, or read about using "millis()" to move and scan and ping.

6

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