Ejaj robot follower

KOD W PROCESSING:

import java.io.*;
import javax.imageio.ImageIO;
import java.awt.image.BufferedImage;

PImage frame;
String url = "http://192.168.4.1/capture";
String baseURL = "http://192.168.4.1/action?go=";

int gridCols = 5;
int gridRows = 3;
int detectedX = -1;
int detectedY = -1;
int prevX = -1;
int prevY = -1;

String lastMove = "";
String lastTurnType = "";
int lastTurnCount = 0;
int impulseDuration = 300;
int movementPower = 1;
int turnPower = 1;
long lastCommandTime = 0;

void setup() {
 size(640, 480);
 textSize(16);
 frameRate(30);
 startImageThread();
}

void draw() {
 background(0);
 if (frame != null) {
   image(frame, 0, 0, width, height);
   analyzeGrid(frame);

   // Rysuj siatkę
   stroke(255, 0, 0, 100);
   for (int i = 1; i < gridCols; i++) line(i * width / gridCols, 0, i * width / gridCols, height);
   for (int j = 1; j < gridRows; j++) line(0, j * height / gridRows, width, j * height / gridRows);
   noStroke();

   if (detectedX >= 0 && detectedY >= 0) {
     fill(255, 0, 0, 80);
     rect(detectedX * width / gridCols, detectedY * height / gridRows,
          width / gridCols, height / gridRows);

     if (detectedX != prevX || detectedY != prevY) {
       String turn = "";
       String move = "";

       if (detectedX <= 0)      turn = repeatCommand("left", turnPower);
       else if (detectedX == 1) turn = repeatCommand("left", 1);
       else if (detectedX == 3) turn = repeatCommand("right", 1);
       else if (detectedX >= 4) turn = repeatCommand("right", turnPower);

       if (detectedY == 0)      move = repeatCommand("forward", movementPower);
       else if (detectedY == 1) move = "stop";
       else if (detectedY == 2) move = repeatCommand("backward", movementPower);

       executeCommand(turn, move);

       prevX = detectedX;
       prevY = detectedY;
     }
   } else {
     executeCommand("", "stop");
     prevX = -1;
     prevY = -1;
   }
 } else {
   fill(255, 0, 0);
   text("Brak obrazu", 20, height / 2);
 }
}

String repeatCommand(String cmd, int times) {
 return cmd.repeat(times);
}

void startImageThread() {
 Thread imageThread = new Thread(() -> {
   while (true) {
     try {
       byte[] raw = loadBytes(url);
       if (raw != null && raw.length > 0) {
         BufferedImage img = ImageIO.read(new ByteArrayInputStream(raw));
         if (img != null) {
           frame = rotate180(convertBufferedImage(img));
         }
       }
     } catch (Exception e) {
       println("Błąd ładowania obrazu: " + e.getMessage());
     }
     delay(200);
   }
 });
 imageThread.start();
}

void sendCommand(String cmd) {
 try {
   loadBytes(baseURL + cmd);
   println("Wysłano komendę: " + cmd);
 } catch (Exception e) {
   println("Błąd HTTP: " + e.getMessage());
 }
}

void executeCommand(String turn, String move) {
 int now = millis();
 String currentTurnType = turn.replaceAll("[^a-z]", "");
 int currentTurnCount = currentTurnType.isEmpty() ? 0 : turn.length() / currentTurnType.length();

 if (!currentTurnType.equals(lastTurnType) || currentTurnCount != lastTurnCount || now - lastCommandTime > impulseDuration) {
   for (char c : turn.toCharArray()) {
     if (c == 'l') sendCommand("left");
     if (c == 'r') sendCommand("right");
     delay(impulseDuration);
   }
   lastTurnType = currentTurnType;
   lastTurnCount = currentTurnCount;
 }

 if (!move.equals(lastMove) || now - lastCommandTime > impulseDuration) {
   for (char c : move.toCharArray()) {
     if (c == 'f') sendCommand("forward");
     if (c == 'b') sendCommand("backward");
     delay(impulseDuration);
   }
   sendCommand("stop");
   lastMove = move;
 }
 lastCommandTime = millis();
}

void analyzeGrid(PImage img) {
 img.loadPixels();
 int cellW = img.width / gridCols;
 int cellH = img.height / gridRows;

 float maxRedRatio = 0;
 detectedX = -1;
 detectedY = -1;

 for (int gy = 0; gy < gridRows; gy++) {
   for (int gx = 0; gx < gridCols; gx++) {
     int redCount = 0;
     int total = cellW * cellH;

     for (int y = gy * cellH; y < (gy + 1) * cellH; y++) {
       for (int x = gx * cellW; x < (gx + 1) * cellW; x++) {
         int idx = y * img.width + x;
         if (idx >= 0 && idx < img.pixels.length) {
           color c = img.pixels[idx];
           float r = red(c), g = green(c), b = blue(c);
           if (r > 100 && r > g + 20 && r > b + 20) redCount++;
         }
       }
     }

     float redRatio = redCount / float(total);
     if (redRatio > maxRedRatio) {
       maxRedRatio = redRatio;
       detectedX = gx;
       detectedY = gy;
     }
   }
 }
}

PImage rotate180(PImage img) {
 PImage flipped = createImage(img.width, img.height, RGB);
 img.loadPixels();
 flipped.loadPixels();
 for (int y = 0; y < img.height; y++) {
   for (int x = 0; x < img.width; x++) {
     int src = x + y * img.width;
     int dst = (img.width - 1 - x) + (img.height - 1 - y) * img.width;
     flipped.pixels[dst] = img.pixels[src];
   }
 }
 flipped.updatePixels();
 return flipped;
}

PImage convertBufferedImage(BufferedImage img) {
 int w = img.getWidth();
 int h = img.getHeight();
 PImage pimg = createImage(w, h, RGB);
 img.getRGB(0, 0, w, h, pimg.pixels, 0, w);
 pimg.updatePixels();
 return pimg;
}

KOD W ARDUINOIDE DO ESP (modyfikacja pliku camera_server.cpp z projektu Sharky):

  #include "config.h"
#include "motor.h"
#include "http.h"

#include "esp_camera.h"
#include "esp_http_server.h"

void motor_forward(int speed);
void motor_backward(int speed);
void motor_left(int speed);
void motor_right(int speed);
void motor_stop(void);

#define PART_BOUNDARY "123456789000000000000987654321"

static const char* _STREAM_CONTENT_TYPE = "multipart/x-mixed-replace;boundary=" PART_BOUNDARY;
static const char* _STREAM_BOUNDARY = "\r\n--" PART_BOUNDARY "\r\n";
static const char* _STREAM_PART = "Content-Type: image/jpeg\r\nContent-Length: %u\r\n\r\n";

httpd_handle_t camera_httpd = NULL;
httpd_handle_t stream_httpd = NULL;

static esp_err_t index_handler(httpd_req_t *req){
 httpd_resp_set_type(req, "text/html");
 return httpd_resp_send(req, (const char *)INDEX_HTML, strlen(INDEX_HTML));
}

static esp_err_t stream_handler(httpd_req_t *req){
   camera_fb_t *fb = NULL;
   struct timeval _timestamp;
   esp_err_t res = ESP_OK;
   size_t _jpg_buf_len = 0;
   uint8_t *_jpg_buf = NULL;
   char *part_buf[128];

   static int64_t last_frame = 0;
   if (!last_frame) {
       last_frame = esp_timer_get_time();
   }

   res = httpd_resp_set_type(req, _STREAM_CONTENT_TYPE);
   if (res != ESP_OK) return res;

   httpd_resp_set_hdr(req, "Access-Control-Allow-Origin", "*");
   httpd_resp_set_hdr(req, "X-Framerate", "60");

   while (true) {
       fb = esp_camera_fb_get();
       if (!fb) {
           ESP_LOGE(TAG, "Camera capture failed");
           res = ESP_FAIL;
       } else {
           _timestamp.tv_sec = fb->timestamp.tv_sec;
           _timestamp.tv_usec = fb->timestamp.tv_usec;

           if (fb->format != PIXFORMAT_JPEG) {
               bool jpeg_converted = frame2jpg(fb, 80, &_jpg_buf, &_jpg_buf_len);
               esp_camera_fb_return(fb);
               fb = NULL;
               if (!jpeg_converted) {
                   ESP_LOGE(TAG, "JPEG compression failed");
                   res = ESP_FAIL;
               }
           } else {
               _jpg_buf_len = fb->len;
               _jpg_buf = fb->buf;
           }
       }

       if (res == ESP_OK)
           res = httpd_resp_send_chunk(req, _STREAM_BOUNDARY, strlen(_STREAM_BOUNDARY));

       if (res == ESP_OK) {
           size_t hlen = snprintf((char *)part_buf, 128, _STREAM_PART, _jpg_buf_len, _timestamp.tv_sec, _timestamp.tv_usec);
           res = httpd_resp_send_chunk(req, (const char *)part_buf, hlen);
       }

       if (res == ESP_OK)
           res = httpd_resp_send_chunk(req, (const char *)_jpg_buf, _jpg_buf_len);

       if (fb) {
           esp_camera_fb_return(fb);
           fb = NULL;
           _jpg_buf = NULL;
       } else if (_jpg_buf) {
           free(_jpg_buf);
           _jpg_buf = NULL;
       }

       if (res != ESP_OK) break;

       int64_t fr_end = esp_timer_get_time();
       int64_t frame_time = fr_end - last_frame;
       last_frame = fr_end;
       frame_time /= 1000;
       ESP_LOGI(TAG, "MJPG: %uB %ums (%.1ffps)",
                (uint32_t)(_jpg_buf_len),
                (uint32_t)frame_time, 1000.0 / (uint32_t)frame_time);
   }

   last_frame = 0;
   return res;
}

static esp_err_t cmd_handler(httpd_req_t *req){
 char*  buf;
 size_t buf_len;
 char variable[32] = {0,};

 buf_len = httpd_req_get_url_query_len(req) + 1;
 if (buf_len > 1) {
   buf = (char*)malloc(buf_len);
   if(!buf){
     httpd_resp_send_500(req);
     return ESP_FAIL;
   }
   if (httpd_req_get_url_query_str(req, buf, buf_len) == ESP_OK) {
     if (httpd_query_key_value(buf, "go", variable, sizeof(variable)) == ESP_OK) {}
     else if(httpd_query_key_value(buf, "Speed", variable, sizeof(variable)) == ESP_OK)
       motor_speed = atoi(variable);
     else if(httpd_query_key_value(buf, "Light", variable, sizeof(variable)) == ESP_OK)
       flash_light = atoi(variable);
     else {
       free(buf);
       httpd_resp_send_404(req);
       return ESP_FAIL;
     }
   } else {
     free(buf);
     httpd_resp_send_404(req);
     return ESP_FAIL;
   }
   free(buf);
 } else {
   httpd_resp_send_404(req);
   return ESP_FAIL;
 }

 int res = 0;
 if(!strcmp(variable, "forward")) motor_forward(motor_speed);
 else if(!strcmp(variable, "left")) motor_left(motor_speed);
 else if(!strcmp(variable, "right")) motor_right(motor_speed);
 else if(!strcmp(variable, "backward")) motor_backward(motor_speed);
 else if(!strcmp(variable, "stop")) motor_stop();
 else res = -1;

 if(res) return httpd_resp_send_500(req);
 httpd_resp_set_hdr(req, "Access-Control-Allow-Origin", "*");
 return httpd_resp_send(req, NULL, 0);
}

static esp_err_t capture_handler(httpd_req_t *req) {
   camera_fb_t *fb = esp_camera_fb_get();
   if (!fb) return httpd_resp_send_500(req);
   httpd_resp_set_type(req, "image/jpeg");
   httpd_resp_send(req, (const char *)fb->buf, fb->len);
   esp_camera_fb_return(fb);
   return ESP_OK;
}

void startCameraServer(){
 httpd_config_t config = HTTPD_DEFAULT_CONFIG();
 config.server_port = 80;

 httpd_uri_t index_uri = { "/", HTTP_GET, index_handler, NULL };
 httpd_uri_t cmd_uri = { "/action", HTTP_GET, cmd_handler, NULL };
 httpd_uri_t stream_uri = { "/stream", HTTP_GET, stream_handler, NULL };
 httpd_uri_t capture_uri = { "/capture", HTTP_GET, capture_handler, NULL };

 if (httpd_start(&camera_httpd, &config) == ESP_OK) {
   httpd_register_uri_handler(camera_httpd, &index_uri);
   httpd_register_uri_handler(camera_httpd, &cmd_uri);
   httpd_register_uri_handler(camera_httpd, &capture_uri);
 }

 config.server_port += 1;
 config.ctrl_port += 1;

 if (httpd_start(&stream_httpd, &config) == ESP_OK) {
   httpd_register_uri_handler(stream_httpd, &stream_uri);
 }
}