Kenan Paralija
Published © CERN-OHL

Tracking and Search Function with ESP32 and arduino robot

Design and Building of a Smart Platform with ESP32 Cam to achieve the Mode Tracking for search and Following the Face

AdvancedFull instructions provided8 hours2,793
Tracking and Search Function with ESP32 and arduino robot

Things used in this project

Hardware components

ESP32 Camera Module Development Board
M5Stack ESP32 Camera Module Development Board
×1

Software apps and online services

Arduino IDE
Arduino IDE

Hand tools and fabrication machines

Multitool, Screwdriver
Multitool, Screwdriver

Story

Read more

Schematics

• pan and tilt stand

Mounting an ESP32-CAM into a pan and tilt platform with Multi-Functional 2WD driving Straight Robot Car

Code

Face_FollowMe23

C/C++
how to achieve the search and follow Mode of the Smart Robot Car and make it follow the target to move forward
//ESP32 camera: face detection
//and body movement
//how to detect faces using the ESP32 and a camera

#include "esp_camera.h"
#include "fd_forward.h"
#include "fb_gfx.h"
#include "soc/soc.h"             // disable brownout problems
#include "soc/rtc_cntl_reg.h"    // disable brownout problems

int  posH;
int  posV;

#define LED_BUILTIN 4 // No #define LED_PIN
 
#define PWDN_GPIO_NUM     32
#define RESET_GPIO_NUM    -1
#define XCLK_GPIO_NUM      0
#define SIOD_GPIO_NUM     26
#define SIOC_GPIO_NUM     27
#define Y9_GPIO_NUM       35
#define Y8_GPIO_NUM       34
#define Y7_GPIO_NUM       39
#define Y6_GPIO_NUM       36
#define Y5_GPIO_NUM       21
#define Y4_GPIO_NUM       19
#define Y3_GPIO_NUM       18
#define Y2_GPIO_NUM        5
#define VSYNC_GPIO_NUM    25
#define HREF_GPIO_NUM     23
#define PCLK_GPIO_NUM     22

//++++++++++++++++++++++++++++++++++++++++++++ function called draw_face_boxes that is used to display a box around a detected face
static void draw_face_boxes(dl_matrix3du_t *image_matrix, box_array_t *boxes)
{
  int x, y, w, h, i, half_width, half_height;  
  fb_data_t fb;
  fb.width = image_matrix->w;
  fb.height = image_matrix->h;
  fb.data = image_matrix->item;
  fb.bytes_per_pixel = 3;
  fb.format = FB_BGR888;
  for (i = 0; i < boxes->len; i++) {

    // finding face centre...
    x = ((int)boxes->box[i].box_p[0]);
    w = (int)boxes->box[i].box_p[2] - x + 1;
    half_width = w / 2;
    //++++++++++++++++++++++++++ Center PAN is 160 +++++++++++++++++++++++++++++++++++++++++++++++
    int face_center_pan = x + half_width; // image frame face centre x co-ordinate

    y = (int)boxes->box[i].box_p[1];
    h = (int)boxes->box[i].box_p[3] - y + 1;
    half_height = h / 2;
    posH =posH + (160 - face_center_pan)/7; //was 6,
    if (posH>170) 
    { 
       Serial2.println("B"); // serial2 -turn LEFT
       delay(100);
       Serial2.println("E"); // stop
       //posH=170;   //event 
       posH=150;
    }
    if (posH<20) 
     {
     Serial2.println("C"); // turn RIGHT
     delay(100);
     Serial2.println("E"); // stop
     //posH=10;    //event 
     posH=30;
    }
    
   Serial.printf("Center detected at %d dots\n", face_center_pan);  
   Serial2.printf("H%d \n", posH);
   Serial.printf("H%d \n", posH);
   
       
   
    //++++++++++++++++++++++++ Center TILT is 120 ++++++++++++++++++++++++++++++++++++++++++++++++++   
    int face_center_tilt = y + half_height;  // image frame face centre y co-ordinate
    posV =posV + (120 - face_center_tilt)/7;  //
    if (posV>130) 
    { 
      posV=130;   //LIMIT UP;
       }
    if (posV<50) 
     {
      posV=50;    //LIMIT DOWN
     }
     Serial2.printf("V%d \n", posV);
     Serial.printf("V%d \n", posV);
 
  } 
  }

 
 
mtmn_config_t mtmn_config = {0};
 // configurations of MTMN that will be used to detect the faces -
 //using the default configurations
 
int noDetection = 0;
  //define a variable that will count how many times we have not detected faces. 
  //We will initialize it with the value zero and increment it every time faces are not detected in a frame.

  //pins for communikation with arduino
  #define RXD2 13 //Rx for serial2
  #define TXD2 15 // Tx for serial2
      
void setup() {
  Serial.begin(115200);
    
   if (!initCamera()) {
 
    Serial.printf("Failed to initialize camera...");
    return;
  }
   
    posH=90;
    posV=90;

  Serial2.begin(9600, SERIAL_8N1, RXD2, TXD2);
  // Serial for ARDUINO auf  Pins Rx>13, TX>15
    delay(1000);
       if(Serial2){
       Serial.println("******Serial2 successfully set up******");
   } 
   Serial2.setDebugOutput(false);
   Serial.println("Face_FollowMe23");
   pinMode (LED_BUILTIN, OUTPUT); // initialize digital pin 4 LED_BUILTIN as an output.
  
  mtmn_config = mtmn_init_config();
 
 
}
 
void loop() {
  camera_fb_t * frame;
   
  frame = esp_camera_fb_get();    
  //call the esp_camera_fb_get function to get an image from the camera

  dl_matrix3du_t *image_matrix = dl_matrix3du_alloc(1, frame->width, frame->height, 3);
    //As output, this function will return a pointer to the allocated matrix struct.
    
    //to convert the captured image to RGB888 format . Sign "->" allows to access elements in Structures
  fmt2rgb888(frame->buf, frame->len, frame->format, image_matrix->item);
 
  esp_camera_fb_return(frame);
  //This function call will allow the image buffer to be reused again, 
 
  box_array_t *boxes = face_detect(image_matrix, &mtmn_config);
  //A box_array_t type value contains face boxes -coordinates: left top, right down, landmarks 
  
 
  if (boxes != NULL) {
    noDetection = 0;
    Serial2.println("E"); // arduino stop
    
    Serial.printf("Faces detected at %d \n", millis());
        
    draw_face_boxes(image_matrix, boxes);   //print command for move or pan camera
    dl_lib_free(boxes->score);
    dl_lib_free(boxes->box);
    dl_lib_free(boxes->landmark);
    dl_lib_free(boxes);
  }
  else
      {    
     noDetection = noDetection+1;
     Serial.printf("Faces not not detected %d times \n", noDetection);

     if (posH <=90) 
     {
    switch(noDetection){
    case 10: Serial2.println("3"); posH=60; break; // execute the corresponding function wenn after 5 times NOdetektion
    case 22: {Serial2.println("E");  //xxxxxxxxx
              posH = 20; Serial2.printf("H%d \n", posH);
              } 
              break;
    case 27: Serial2.println("2"); break;//
    case 31: {Serial2.println("E");    //xxxxxxxxx
              posH = 60; Serial2.printf("H%d \n", posH); 
              }  
              break;  
    case 36: Serial2.println("2"); break;//
    case 40: {
              Serial2.println("E");   //xxxxxxxxx
              posH = 91; Serial2.printf("H%d \n", posH);
              noDetection = 0; // stop
              }
              break;
       
    default:  break;
     }
      }
      
     if (posH>90) 
     {
    switch(noDetection){
    case 10: Serial2.println("2"); posH=140; break;   // execute the corresponding function wenn after 5 times NOdetektion
    case 22: {Serial2.println("E");   //xxxxxxxxx
              posH = 160; Serial2.printf("H%d \n", posH); 
              }
              break;
    case 27: Serial2.println("3"); break;//
    case 31: {Serial2.println("E");  //xxxxxxxxx
              posH = 120; Serial2.printf("H%d \n", posH);
              }
              break;
    case 36: Serial2.println("3"); break;//
    case 38: {Serial2.println("E");  //xxxxxxxxx
              posH = 90; Serial2.printf("H%d \n", posH);
              noDetection = 0; // stop
              }
              break;
    //case 56: digitalWrite(LED_BUILTIN, HIGH);   break; //event TURN on
    //ev. followObjekt(); ObjektSearch;
    
    default:  break;
     }
    }
         
    Serial.println("E? ---------------"); //boxes false - stop
        
   }
 
  dl_matrix3du_free(image_matrix);
  
 
}

//*******************************************************************
bool initCamera() {
 
  camera_config_t config;
 
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer = LEDC_TIMER_0;
  config.pin_d0 = Y2_GPIO_NUM;
  config.pin_d1 = Y3_GPIO_NUM;
  config.pin_d2 = Y4_GPIO_NUM;
  config.pin_d3 = Y5_GPIO_NUM;
  config.pin_d4 = Y6_GPIO_NUM;
  config.pin_d5 = Y7_GPIO_NUM;
  config.pin_d6 = Y8_GPIO_NUM;
  config.pin_d7 = Y9_GPIO_NUM;
  config.pin_xclk = XCLK_GPIO_NUM;
  config.pin_pclk = PCLK_GPIO_NUM;
  config.pin_vsync = VSYNC_GPIO_NUM;
  config.pin_href = HREF_GPIO_NUM;
  config.pin_sscb_sda = SIOD_GPIO_NUM;
  config.pin_sscb_scl = SIOC_GPIO_NUM;
  config.pin_pwdn = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;
  config.xclk_freq_hz = 20000000;
  config.pixel_format = PIXFORMAT_JPEG;
  config.frame_size = FRAMESIZE_QVGA;
  config.jpeg_quality = 10;
  config.fb_count = 1;
 
  esp_err_t result = esp_camera_init(&config);
 
  if (result != ESP_OK) {
    return false;
  }
 
  return true;
}

Credits

Kenan Paralija

Kenan Paralija

8 projects • 7 followers
I started my career by designing analog and digital circuits, moved to telecommunication and finally as a IT/TK lecturer.

Comments

Related channels and tags
  • M5Stack
  • Machine Learning & AI
  • Robotics