#include <WebServer.h>

#include <WiFi.h>

#include <PubSubClient.h>

#include <esp32cam.h>

const char* WIFI_SSID = "wifi的名称";

const char* WIFI_PASS = "wifi的密码";

WebServer server(80);

static auto loRes = esp32cam::Resolution::find(320, 240);

static auto hiRes = esp32cam::Resolution::find(800, 600);

void callback(char* topic, byte* payload, unsigned int length) ;

bool stopEngine = true;

//初始化mqtt类对象

WiFiClient espClient;

PubSubClient mqtt_client(espClient);

const char* mqttServer = "broker.emqx.io";

const int mqttPort = 1883;

const char* mqttUser = "";

const char* mqttPassword = "";

int servo_y = 8;

int servo_z = 9;

int servo_y_pin = 14;

int servo_z_pin = 13;

int pos_z = 90 , pos_y = 90;

void serveJpg()

{

  auto frame = esp32cam::capture();

  if (frame == nullptr) {

    Serial.println("CAPTURE FAIL");

    server.send(503, "", "");

    return;

  }

  //Serial.printf("CAPTURE OK %dx%d %db\n", frame->getWidth(), frame->getHeight(),

  //static_cast<int>(frame->size()));

  server.setContentLength(frame->size());

  server.send(200, "image/jpeg");

  WiFiClient client = server.client();

  frame->writeTo(client);

}

void handleJpgHi()

{

  if (!esp32cam::Camera.changeResolution(hiRes)) {

    Serial.println("SET-HI-RES FAIL");

  }

  serveJpg();

}

int calculatePWM( int degree)

{

  const float deadZone = 6.4;

  const float max = 32;

  if (degree < 0)

    degree = 0;

  if (degree > 180)

    degree = 180;

  return ( int)(((max - deadZone) / 180) * degree + deadZone);

}

void mqtt_connet(){

    mqtt_client.setServer(mqttServer,mqttPort);

    mqtt_client.setCallback(callback);

      while (!mqtt_client.connected()) {

    Serial.println("Connecting to MQTT...");

    if (mqtt_client.connect(mqttServer, mqttUser, mqttPassword )) {

      Serial.println("connected");

    } else {

      Serial.print("failed with state ");

      Serial.print(mqtt_client.state());

      delay(2000);

    }

  }

  mqtt_client.subscribe("POSITION");

}

void callback(char* topic, byte* payload, unsigned int length) {

  String payloadStr = "";

  for (int i=0; i<length; i++) {

    payloadStr += (char)payload[i];

  }

  Serial.println(payloadStr);

  if(payloadStr.equals("RIGHT")){

    pos_z --;

  }else if(payloadStr.equals("LEFT")){

    pos_z ++;

  }

  if(payloadStr.equals("UP")){

    pos_y --;

  }else if(payloadStr.equals("DOWN")){

    pos_y ++;

  }

  if(pos_z >= 180) pos_z = 180;

  if(pos_z <= 0) pos_z = 0;

  if(pos_y >= 180) pos_y = 180;

  if(pos_y <= 0) pos_y = 0;

  Serial.printf("pos_z: %d pos_y: %d \n",pos_z,pos_y);

  ledcWrite(servo_z,calculatePWM(pos_z));

  ledcWrite(servo_y,calculatePWM(pos_y));

}

void setup()

{

  Serial.begin(115200);

  ledcSetup(servo_y, 50, 8);

  ledcSetup(servo_z, 50, 8);

  ledcAttachPin(servo_y_pin, servo_y);

  ledcAttachPin(servo_z_pin, servo_z); 

  {

    using namespace esp32cam;

    Config cfg;

    cfg.setPins(pins::AiThinker);

    cfg.setResolution(hiRes);

    cfg.setBufferCount(2);

    cfg.setJpeg(80);

    bool ok = Camera.begin(cfg);

    Serial.println(ok ? "CAMERA OK" : "CAMERA FAIL");

  }

  WiFi.persistent(false);

  WiFi.mode(WIFI_STA);

  WiFi.begin(WIFI_SSID, WIFI_PASS);

  while (WiFi.status() != WL_CONNECTED) {

    delay(500);

  }

  Serial.print("http://");

  Serial.println(WiFi.localIP());

  Serial.println("  /cam-hi.jpg");

  server.on("/cam-hi.jpg", handleJpgHi);

  mqtt_connet();

  server.begin();

}

void loop()

{

  server.handleClient();

  mqtt_client.loop();

}

import cv2

import time

import urllib.request

import numpy as np

import paho.mqtt.client as mqtt

url='http://192.168.0.106/cam-hi.jpg'

cv2.namedWindow("Berhasil", cv2.WINDOW_AUTOSIZE)

face=cv2.CascadeClassifier('haarcascade_frontalface_default.xml')

mqttBroker = "broker.emqx.io"

client = mqtt.Client("Cleaning-Robot",clean_session=True,userdata=None)

client.connect(mqttBroker,1883)

while True:

    imgresponse=urllib.request.urlopen(url)

    start = time.time()

    imgnp=np.array(bytearray(imgresponse.read()),dtype=np.uint8)

    img=cv2.imdecode(imgnp,-1)

    rows , cols , _ = img.shape

    conter_x = (int)(rows / 2)

    conter_y = (int)(cols / 2)

    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

    faces = face.detectMultiScale(gray,1.3,5)

    for (x,y,w,h) in faces:

        faces_conter_x = (int)(x + (w / 2))

        faces_conter_y = (int)(y + (h / 2))

        cv2.rectangle(img,(x,y), (x+w,y+h), (0,255,120),2)

        cv2.putText(img,"face",(w+x,y+h),cv2.FONT_HERSHEY_PLAIN,2,(0,255,255),2)

        if(faces_conter_x > conter_x):

            client.publish("POSITION","RIGHT")

            str_x = str(conter_x - faces_conter_x)

            print("RIGHT" + str_x)

        elif(faces_conter_x < conter_x):

            client.publish("POSITION","LEFT")

            str_x = str(faces_conter_y - conter_y)

            print("LEFT:" + str_x)

        if(faces_conter_y > conter_y):

            client.publish("POSITION","DOWN")

            str_y = str(conter_y - faces_conter_y)

            print("DOWN:" + str_y)

        elif(faces_conter_y < conter_y):

            client.publish("POSITION","UP")

            str_y = str(faces_conter_y - conter_y)

            print("UP:" + str_y)

    end = time.time()

    seconds = end - start  # 处理一帧所用的时间

    fps = 1 / seconds  # 一秒钟可以处理多少帧

    fps = "%.2f fps"%fps

    cv2.putText(img, fps, (5,50 ), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 1)

    cv2.imshow("Berhasil",img)

    key=cv2.waitKey(30) & 0xff

    if key==27:

        break

cv2.destroyAllWindows