Hi I want to make a pan tilt camera using esp32 camera (ai thinker) and a FTDI. I want the servos to move/track the object according the position (x,y)
I took the existing edge impulse code and I added( in order to find the center point )a line that calculates the center using the height and width of bounding_boxes
But the servos the don’t move accordingly they move a bit off
#if EI_CLASSIFIER_OBJECT_DETECTION == 1
bool bb_found = result.bounding_boxes[0].value > 0;
for (size_t ix = 0; ix < result.bounding_boxes_count; ix++) {
auto bb = result.bounding_boxes[ix];
if (bb.value == 0) {
continue;
}
centerX=bb.x+(bb.width/2);
centerY=bb.y+(bb.height/2);
servoX.write(centerX);
servoY.write(centerY);
ei_printf(" %s (%f) [ x: %u, y: %u, width: %u, height: %u ]\n", bb.label, bb.value, bb.x, bb.y, bb.width, bb.height);
```
The output is :
Predictions (DSP: 2 ms., Classification: 131 ms., Anomaly: 0 ms.):
No objects found
Predictions (DSP: 2 ms., Classification: 131 ms., Anomaly: 0 ms.):
No objects found
Predictions (DSP: 2 ms., Classification: 132 ms., Anomaly: 0 ms.):
No objects found
Predictions (DSP: 2 ms., Classification: 132 ms., Anomaly: 0 ms.):
No objects found
Predictions (DSP: 2 ms., Classification: 131 ms., Anomaly: 0 ms.):
No objects found
Predictions (DSP: 2 ms., Classification: 132 ms., Anomaly: 0 ms.):
Son Goku (0.902344) [ x: 16, y: 16, width: 8, height: 16 ]
Predictions (DSP: 2 ms., Classification: 132 ms., Anomaly: 0 ms.):
Son Goku (0.621094) [ x: 16, y: 16, width: 16, height: 8 ]
Predictions (DSP: 2 ms., Classification: 132 ms., Anomaly: 0 ms.):
Son Goku (0.855469) [ x: 24, y: 16, width: 8, height: 16 ]
Predictions (DSP: 2 ms., Classification: 132 ms., Anomaly: 0 ms.):
Son Goku (0.687500) [ x: 24, y: 16, width: 8, height: 16 ]
Predictions (DSP: 2 ms., Classification: 132 ms., Anomaly: 0 ms.):
Son Goku (0.773438) [ x: 8, y: 8, width: 16, height: 16 ]
Predictions (DSP: 2 ms., Classification: 132 ms., Anomaly: 0 ms.):
Son Goku (0.828125) [ x: 8, y: 8, width: 16, height: 16 ]
Predictions (DSP: 2 ms., Classification: 132 ms., Anomaly: 0 ms.):
Son Goku (0.835938) [ x: 8, y: 8, width: 16, height: 16 ]
Predictions (DSP: 2 ms., Classification: 132 ms., Anomaly: 0 ms.):
Son Goku (0.882812) [ x: 8, y: 16, width: 16, height: 8 ]
Predictions (DSP: 2 ms., Classification: 132 ms., Anomaly: 0 ms.):
Son Goku (0.835938) [ x: 16, y: 16, width: 8, height: 8 ]
Bounding box information (x, y, width, height) are given in pixels. So, when you calculate center coordinates (centerX and centerY), those are pixels (from 0 to whatever the width/height of your image is). Assuming you are using the Arduino servo library, the Servo.write() method expects an angle in degrees (0 to 180). You need to translate center coordinates in pixels to an angle to pass to your servos.
What behavior are you seeing from the servo and what are you expecting it to do?
Also, keep in mind if there is more than one bounding box, you will give conflicting instructions to your servos, as you attempt to move them for every bounding box (as given by the for loop).
Tilt servo is going downwards I check the code if I put wrong the pan movement into tilt
I provide you with a video and the code!
In the link the are 2 videos
/* Edge Impulse Arduino examples
* Copyright (c) 2022 EdgeImpulse Inc.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
/* Includes ---------------------------------------------------------------- */
#include "ESP32_inferencing.h"
#include "edge-impulse-sdk/dsp/image/image.hpp"
#include <ESP32Servo.h>
#include "esp_camera.h"
#define LED1 4
#define SERVO_PIN_1 14 // GPIO14
#define SERVO_PIN_2 13 // GPIO13
Servo servoPan;
Servo servoTilt;
// Select camera model - find more camera models in camera_pins.h file here
// https://github.com/espressif/arduino-esp32/blob/master/libraries/ESP32/examples/Camera/CameraWebServer/camera_pins.h
//#define CAMERA_MODEL_ESP_EYE // Has PSRAM
#define CAMERA_MODEL_AI_THINKER // Has PSRAM
#if defined(CAMERA_MODEL_ESP_EYE)
#define PWDN_GPIO_NUM -1
#define RESET_GPIO_NUM -1
#define XCLK_GPIO_NUM 4
#define SIOD_GPIO_NUM 18
#define SIOC_GPIO_NUM 23
#define Y9_GPIO_NUM 36
#define Y8_GPIO_NUM 37
#define Y7_GPIO_NUM 38
#define Y6_GPIO_NUM 39
#define Y5_GPIO_NUM 35
#define Y4_GPIO_NUM 14
#define Y3_GPIO_NUM 13
#define Y2_GPIO_NUM 34
#define VSYNC_GPIO_NUM 5
#define HREF_GPIO_NUM 27
#define PCLK_GPIO_NUM 25
#elif defined(CAMERA_MODEL_AI_THINKER)
#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
#else
#error "Camera model not selected"
#endif
/* Constant defines -------------------------------------------------------- */
#define EI_CAMERA_RAW_FRAME_BUFFER_COLS 320
#define EI_CAMERA_RAW_FRAME_BUFFER_ROWS 240
#define EI_CAMERA_FRAME_BYTE_SIZE 3
/* Private variables ------------------------------------------------------- */
static bool debug_nn = false; // Set this to true to see e.g. features generated from the raw signal
static bool is_initialised = false;
uint8_t *snapshot_buf; //points to the output of the capture
static camera_config_t camera_config = {
.pin_pwdn = PWDN_GPIO_NUM,
.pin_reset = RESET_GPIO_NUM,
.pin_xclk = XCLK_GPIO_NUM,
.pin_sscb_sda = SIOD_GPIO_NUM,
.pin_sscb_scl = SIOC_GPIO_NUM,
.pin_d7 = Y9_GPIO_NUM,
.pin_d6 = Y8_GPIO_NUM,
.pin_d5 = Y7_GPIO_NUM,
.pin_d4 = Y6_GPIO_NUM,
.pin_d3 = Y5_GPIO_NUM,
.pin_d2 = Y4_GPIO_NUM,
.pin_d1 = Y3_GPIO_NUM,
.pin_d0 = Y2_GPIO_NUM,
.pin_vsync = VSYNC_GPIO_NUM,
.pin_href = HREF_GPIO_NUM,
.pin_pclk = PCLK_GPIO_NUM,
//XCLK 20MHz or 10MHz for OV2640 double FPS (Experimental)
.xclk_freq_hz = 20000000,
.ledc_timer = LEDC_TIMER_0,
.ledc_channel = LEDC_CHANNEL_0,
.pixel_format = PIXFORMAT_JPEG, //YUV422,GRAYSCALE,RGB565,JPEG
.frame_size = FRAMESIZE_QVGA, //QQVGA-UXGA Do not use sizes above QVGA when not JPEG
.jpeg_quality = 12, //0-63 lower number means higher quality
.fb_count = 1, //if more than one, i2s runs in continuous mode. Use only with JPEG
.fb_location = CAMERA_FB_IN_PSRAM,
.grab_mode = CAMERA_GRAB_WHEN_EMPTY,
};
/* Function definitions ------------------------------------------------------- */
bool ei_camera_init(void);
void ei_camera_deinit(void);
bool ei_camera_capture(uint32_t img_width, uint32_t img_height, uint8_t *out_buf) ;
int centerX = 0;
int centerY = 0;
int servo_min_angle = 0; // Ελάχιστη γωνία του servo
int servo_max_angle = 180; // Μέγιστη γωνία του servo
int image_width = 48; // Πλάτος της εικόνας
int image_height = 48; // Ύψος της εικόνας
/**
* @brief Arduino setup function
*/
void setup()
{
servoTilt.attach(SERVO_PIN_1);
servoPan.attach(SERVO_PIN_2);
pinMode(LED1,OUTPUT);
// put your setup code here, to run once:
Serial.begin(115200);
//comment out the below line to start inference immediately after upload
// move all servos to position 0
servoPan.write(centerX);
servoTilt.write(centerY);
while (!Serial);
Serial.println("Edge Impulse Inferencing Demo");
if (ei_camera_init() == false) {
ei_printf("Failed to initialize Camera!\r\n");
}
else {
ei_printf("Camera initialized\r\n");
}
ei_printf("\nStarting continious inference in 2 seconds...\n");
ei_sleep(2000);
}
/**
* @brief Get data and run inferencing
*
* @param[in] debug Get debug info if true
*/
void loop()
{
// instead of wait_ms, we'll wait on the signal, this allows threads to cancel us...
if (ei_sleep(5) != EI_IMPULSE_OK) {
return;
}
snapshot_buf = (uint8_t*)malloc(EI_CAMERA_RAW_FRAME_BUFFER_COLS * EI_CAMERA_RAW_FRAME_BUFFER_ROWS * EI_CAMERA_FRAME_BYTE_SIZE);
// check if allocation was successful
if(snapshot_buf == nullptr) {
ei_printf("ERR: Failed to allocate snapshot buffer!\n");
return;
}
ei::signal_t signal;
signal.total_length = EI_CLASSIFIER_INPUT_WIDTH * EI_CLASSIFIER_INPUT_HEIGHT;
signal.get_data = &ei_camera_get_data;
if (ei_camera_capture((size_t)EI_CLASSIFIER_INPUT_WIDTH, (size_t)EI_CLASSIFIER_INPUT_HEIGHT, snapshot_buf) == false) {
ei_printf("Failed to capture image\r\n");
free(snapshot_buf);
return;
}
// Run the classifier
ei_impulse_result_t result = { 0 };
EI_IMPULSE_ERROR err = run_classifier(&signal, &result, debug_nn);
if (err != EI_IMPULSE_OK) {
ei_printf("ERR: Failed to run classifier (%d)\n", err);
return;
}
// print the predictions
ei_printf("Predictions (DSP: %d ms., Classification: %d ms., Anomaly: %d ms.): \n",
result.timing.dsp, result.timing.classification, result.timing.anomaly);
#if EI_CLASSIFIER_OBJECT_DETECTION == 1
bool bb_found = result.bounding_boxes[0].value > 0;
for (size_t ix = 0; ix < result.bounding_boxes_count; ix++) {
auto bb = result.bounding_boxes[ix];
if (bb.value == 0) {
//digitalWrite(LED1,LOW);
continue;
}
int servoPan_angle = map(bb.x, 0, image_width, servo_min_angle, servo_max_angle);
int servoTilt_angle = map(bb.y, 0, image_height, servo_min_angle, servo_max_angle);
servoPan.write(servoPan_angle);
servoTilt.write(servoTilt_angle);
//try and error ..testing..
//centerX=bb.x+(bb.width/2);
// centerY=bb.y+(bb.height/2);
//printf("KEDRO" , centerX,centerY);
//digitalWrite(LED1,HIGH);
//servoX.write(centerX);
//servoY.write(centerY);
ei_printf(" %s (%f) [ x: %u, y: %u, width: %u, height: %u ]\n", bb.label, bb.value, bb.x, bb.y, bb.width, bb.height);
}
if (!bb_found) {
ei_printf(" No objects found\n");
//digitalWrite(LED1,LOW);
}
#else
for (size_t ix = 0; ix < EI_CLASSIFIER_LABEL_COUNT; ix++) {
ei_printf(" %s: %.5f\n", result.classification[ix].label,
result.classification[ix].value);
}
#endif
#if EI_CLASSIFIER_HAS_ANOMALY == 1
ei_printf(" anomaly score: %.3f\n", result.anomaly);
#endif
free(snapshot_buf);
}
/**
* @brief Setup image sensor & start streaming
*
* @retval false if initialisation failed
*/
bool ei_camera_init(void) {
if (is_initialised) return true;
#if defined(CAMERA_MODEL_ESP_EYE)
pinMode(13, INPUT_PULLUP);
pinMode(14, INPUT_PULLUP);
#endif
//initialize the camera
esp_err_t err = esp_camera_init(&camera_config);
if (err != ESP_OK) {
Serial.printf("Camera init failed with error 0x%x\n", err);
return false;
}
sensor_t * s = esp_camera_sensor_get();
// initial sensors are flipped vertically and colors are a bit saturated
if (s->id.PID == OV3660_PID) {
s->set_vflip(s, 1); // flip it back
s->set_brightness(s, 1); // up the brightness just a bit
s->set_saturation(s, 0); // lower the saturation
}
#if defined(CAMERA_MODEL_M5STACK_WIDE)
s->set_vflip(s, 1);
s->set_hmirror(s, 1);
#elif defined(CAMERA_MODEL_ESP_EYE)
s->set_vflip(s, 1);
s->set_hmirror(s, 1);
s->set_awb_gain(s, 1);
#endif
is_initialised = true;
return true;
}
/**
* @brief Stop streaming of sensor data
*/
void ei_camera_deinit(void) {
//deinitialize the camera
esp_err_t err = esp_camera_deinit();
if (err != ESP_OK)
{
ei_printf("Camera deinit failed\n");
return;
}
is_initialised = false;
return;
}
/**
* @brief Capture, rescale and crop image
*
* @param[in] img_width width of output image
* @param[in] img_height height of output image
* @param[in] out_buf pointer to store output image, NULL may be used
* if ei_camera_frame_buffer is to be used for capture and resize/cropping.
*
* @retval false if not initialised, image captured, rescaled or cropped failed
*
*/
bool ei_camera_capture(uint32_t img_width, uint32_t img_height, uint8_t *out_buf) {
bool do_resize = false;
if (!is_initialised) {
ei_printf("ERR: Camera is not initialized\r\n");
return false;
}
camera_fb_t *fb = esp_camera_fb_get();
if (!fb) {
ei_printf("Camera capture failed\n");
return false;
}
bool converted = fmt2rgb888(fb->buf, fb->len, PIXFORMAT_JPEG, snapshot_buf);
esp_camera_fb_return(fb);
if(!converted){
ei_printf("Conversion failed\n");
return false;
}
if ((img_width != EI_CAMERA_RAW_FRAME_BUFFER_COLS)
|| (img_height != EI_CAMERA_RAW_FRAME_BUFFER_ROWS)) {
do_resize = true;
}
if (do_resize) {
ei::image::processing::crop_and_interpolate_rgb888(
out_buf,
EI_CAMERA_RAW_FRAME_BUFFER_COLS,
EI_CAMERA_RAW_FRAME_BUFFER_ROWS,
out_buf,
img_width,
img_height);
}
return true;
}
static int ei_camera_get_data(size_t offset, size_t length, float *out_ptr)
{
// we already have a RGB888 buffer, so recalculate offset into pixel index
size_t pixel_ix = offset * 3;
size_t pixels_left = length;
size_t out_ptr_ix = 0;
while (pixels_left != 0) {
out_ptr[out_ptr_ix] = (snapshot_buf[pixel_ix] << 16) + (snapshot_buf[pixel_ix + 1] << 8) + snapshot_buf[pixel_ix + 2];
// go to the next pixel
out_ptr_ix++;
pixel_ix+=3;
pixels_left--;
}
// and done!
return 0;
}
#if !defined(EI_CLASSIFIER_SENSOR) || EI_CLASSIFIER_SENSOR != EI_CLASSIFIER_SENSOR_CAMERA
#error "Invalid model for current sensor"
#endif
My guess is that you’re still moving the servo far more than necessary. You’re trying to map a pixel (x, y) location to the full range of servo motion, e.g. 0 to 180 degrees. So, if your Goku is at the top of the frame (y=0), the servo will try to move all the way to 0 deg. In all likelihood, that will move your camera too far and Goku will be out of the frame.
I recommend taking an iterative approach. If the center of the Goku bounding box (x, y) is above the center of the frame (image_width / 2, image_height / 2), then just move the camera up a little. Same thing for if Goku is below the middle point. You should also have a “dead zone” where the camera doesn’t move at all (start with something like 10 pixels in either direction):