Simple Face search Function with ESP32Cam

//ESP32 camera: face detection
//NO body movement, Servo on ESP32
//how to detect faces using the ESP32cam
//https://randomnerdtutorials.com/esp32-servo-motor-web-server-arduino-ide/
//It can detect a human face with the model MTMN.
//The output is the human face coordinates if there is one in the image.
//left top, right down, landmarks

//ESP32 Camera Driver, Camera OV2640/1600 x 1200/Len Size:1/4"

#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
#include <ESP32Servo.h>

//***** Servo def
#define DUMMY_SERVO1_PIN 12     //We need to create 2 dummy servos.
#define DUMMY_SERVO2_PIN 13     //So that ESP32Servo library does not interfere with pwm channel and timer used by esp32 camera.
  
#define PAN_PIN 14
#define TILT_PIN 15


Servo dummyServo1;
Servo dummyServo2;
Servo panServo;
Servo tiltServo;
// Published values for SG90 servos; adjust if needed
int minUs = 500;
int maxUs = 2400;
//**** Servo the end

int  posH;  //Position Panoram
int  posV;  //Position Tilt
int altPosH;
bool toNul; //servo moves to Null

//#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

    // definition of servo move
    altPosH=posH;
    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 ((altPosH-posH)>0) 
    { 
      //+++++++++
     toNul=false;  //// servo move to 0 grad
    }
    else
    {
        toNul=true; // servo move to plus 180
    }
    
    altPosH=posH;
    
    if (posH>170) 
    { 
      //+++++++++
      posH=170;
    }
    if (posH<20) 
     {
      //+++++++++
     posH=20;
    }
    
    Serial.printf("Center detected at %d dots\n", face_center_pan);
    panServo.write(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
     }
     
      tiltServo.write(posV);
     Serial.printf("V%d \n", posV);
    //delay(100);
  } 
  }

 
 
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.

  
      
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******");   } 
  
   
   Serial.println("Face_FollowMe24-1");
   //pinMode (LED_BUILTIN, OUTPUT); // initialize digital pin 4 LED_BUILTIN as an output.
  
  mtmn_config = mtmn_init_config();
  //****** Servo
  panServo.setPeriodHertz(50);      // Standard 50hz servo
  tiltServo.setPeriodHertz(50);      // Standard 50hz servo
  
  panServo.attach(PAN_PIN, minUs, maxUs);
  tiltServo.attach(TILT_PIN, minUs, maxUs);
  panServo.write(posH); //pan angle 90 start
  tiltServo.write(posV);  //tilt angle 90 start
  
  //******** The End
 }


 
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;
    
    
    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);
    //  As a timer for the search function, we use a variable "noDetection", 
        //which accumulates with each unsuccessful attempt at face detection.
        switch(noDetection){
    case 10:  Serial.printf("Case10 Nodetected at %d \n", millis()); break; // execute the corresponding function wenn after 5 times NOdetektion
  
    case 40: {
              Serial.printf("Case40 Nodetected at %d \n", millis());   //xxxxxxxxx
              noDetection = 0; // stop
              }
              break;
       
    default:  break;
     }
     if (toNul) 
     //block of code to be executed if the condition is true
     {
      posH++;
      //  posH++; speed search
      if (posH>170) 
     {
      //+++++++++
     posH=170;
     posV=50;
     tiltServo.write(posV);
     toNul=false;
    }
      panServo.write(posH);
      }
      
     else 
     {
    posH--;
    // //  posH--; speed search
    if (posH<10) 
    { 
      //+++++++++
      posH=10;
      posV=90;
      tiltServo.write(posV);
      toNul=true;
    }
    panServo.write(posH);
    }
         
    
        
   }
   //
 
  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;
}