ESP32-cam with servo control won't work. Arduino IDE - esp32

Question

I've an ESP32-cam and want to controll a servo. My code compiled without error but doesn't work.
There are two files involved, a .ino and a .cpp. The .ino seems to work right because the servo contributes to his start position 90° at startup but When i want to turn it right or left, nothing happens. This part is controlled in the .cpp and i've spend a lot of time to figure out what went wrong here without success. Any help is highly appreciated.
.ino
#include "esp_camera.h"
#include <WiFiMulti.h>
#include <Servo.h>
Servo myservo;
WiFiMulti wifiMulti;
int ledPin = 4;
#define 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
// GPIO Setting
int servoPin = 2;
int posDegrees = 90; //servo initial position 90°
int posDegreesStep = 30;
extern String WiFiAddr ="";
void startCameraServer();
void setup() {
Serial.begin(115200);
Serial.setDebugOutput(true);
Serial.println();
myservo.attach(servoPin, 2, 0, 180); //(pin, channel, min, max) degrees
myservo.write(posDegrees);
pinMode(servoPin , OUTPUT );
pinMode(ledPin, OUTPUT);
digitalWrite(ledPin, LOW);
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;
//init with high specs to pre-allocate larger buffers
if(psramFound()){
config.frame_size = FRAMESIZE_SXGA;
config.jpeg_quality = 10;
config.fb_count = 2;
} else {
config.frame_size = FRAMESIZE_SVGA;
config.jpeg_quality = 12;
config.fb_count = 1;
}
// camera init
esp_err_t err = esp_camera_init(&config);
if (err != ESP_OK) {
Serial.printf("Camera init failed with error 0x%x", err);
return;
}
//drop down frame size for higher initial frame rate
sensor_t * s = esp_camera_sensor_get();
s->set_framesize(s, FRAMESIZE_CIF);
wifiMulti.addAP("ssid_from_AP_1", "your_password_for_AP_1");
wifiMulti.addAP("ssid_from_AP_3", "your_password_for_AP_2");
//wifiMulti.addAP("ssid_from_AP_3", "your_password_for_AP_3");
Serial.println("Connecting Wifi...");
if (wifiMulti.run() == WL_CONNECTED) {
Serial.println("");
Serial.println("WiFi connected");
Serial.print("IP address: ");
Serial.println(WiFi.localIP());
Serial.print("SSID: ");
Serial.println(WiFi.SSID());
}
startCameraServer();
Serial.print("Camera Ready!");
Serial.print(WiFi.localIP());
WiFiAddr = WiFi.localIP().toString();
Serial.println("' to connect");
}
void wifireconnect() {
if (wifiMulti.run() != WL_CONNECTED) {
Serial.println("WiFi not connected!");
delay(500);
WiFi.disconnect();
Serial.println("\nReboot");
ESP.restart();
}
}
void loop() {
wifireconnect();
}
.cpp
#include "esp_http_server.h"
#include "esp_timer.h"
#include "esp_camera.h"
#include "img_converters.h"
#include "camera_index.h"
#include "Arduino.h"
#include "Servo.h"
extern int servoPin;
extern int posDegrees;
extern int posDegreesStep;
extern String WiFiAddr;
typedef struct {
size_t size;
size_t index;
size_t count;
int sum;
int * values;
} ra_filter_t;
typedef struct {
httpd_req_t *req;
size_t len;
} jpg_chunking_t;
#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";
static ra_filter_t ra_filter;
httpd_handle_t stream_httpd = NULL;
httpd_handle_t camera_httpd = NULL;
static ra_filter_t * ra_filter_init(ra_filter_t * filter, size_t sample_size) {
memset(filter, 0, sizeof(ra_filter_t));
filter->values = (int *)malloc(sample_size * sizeof(int));
if (!filter->values) {
return NULL;
}
memset(filter->values, 0, sample_size * sizeof(int));
filter->size = sample_size;
return filter;
}
static int ra_filter_run(ra_filter_t * filter, int value) {
if (!filter->values) {
return value;
}
filter->sum -= filter->values[filter->index];
filter->values[filter->index] = value;
filter->sum += filter->values[filter->index];
filter->index++;
filter->index = filter->index % filter->size;
if (filter->count < filter->size) {
filter->count++;
}
return filter->sum / filter->count;
}
static size_t jpg_encode_stream(void * arg, size_t index, const void* data, size_t len) {
jpg_chunking_t *j = (jpg_chunking_t *)arg;
if (!index) {
j->len = 0;
}
if (httpd_resp_send_chunk(j->req, (const char *)data, len) != ESP_OK) {
return 0;
}
j->len += len;
return len;
}
static esp_err_t capture_handler(httpd_req_t *req) {
camera_fb_t * fb = NULL;
esp_err_t res = ESP_OK;
int64_t fr_start = esp_timer_get_time();
fb = esp_camera_fb_get();
if (!fb) {
Serial.printf("Camera capture failed");
httpd_resp_send_500(req);
return ESP_FAIL;
}
httpd_resp_set_type(req, "image/jpeg");
httpd_resp_set_hdr(req, "Content-Disposition", "inline; filename=capture.jpg");
size_t fb_len = 0;
if (fb->format == PIXFORMAT_JPEG) {
fb_len = fb->len;
res = httpd_resp_send(req, (const char *)fb->buf, fb->len);
} else {
jpg_chunking_t jchunk = {req, 0};
res = frame2jpg_cb(fb, 80, jpg_encode_stream, &jchunk) ? ESP_OK : ESP_FAIL;
httpd_resp_send_chunk(req, NULL, 0);
fb_len = jchunk.len;
}
esp_camera_fb_return(fb);
int64_t fr_end = esp_timer_get_time();
Serial.printf("JPG: %uB %ums", (uint32_t)(fb_len), (uint32_t)((fr_end - fr_start) / 1000));
return res;
}
static esp_err_t stream_handler(httpd_req_t *req) {
camera_fb_t * fb = NULL;
esp_err_t res = ESP_OK;
size_t _jpg_buf_len = 0;
uint8_t * _jpg_buf = NULL;
char * part_buf[64];
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;
}
while (true) {
fb = esp_camera_fb_get();
if (!fb) {
Serial.printf("Camera capture failed");
res = ESP_FAIL;
} else {
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) {
Serial.printf("JPEG compression failed");
res = ESP_FAIL;
}
} else {
_jpg_buf_len = fb->len;
_jpg_buf = fb->buf;
}
}
if (res == ESP_OK) {
size_t hlen = snprintf((char *)part_buf, 64, _STREAM_PART, _jpg_buf_len);
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 (res == ESP_OK) {
res = httpd_resp_send_chunk(req, _STREAM_BOUNDARY, strlen(_STREAM_BOUNDARY));
}
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;
uint32_t avg_frame_time = ra_filter_run(&ra_filter, frame_time);
//Serial.printf("MJPG: %uB %ums (%.1ffps), AVG: %ums (%.1ffps)"
// ,(uint32_t)(_jpg_buf_len),
// (uint32_t)frame_time, 1000.0 / (uint32_t)frame_time,
// avg_frame_time, 1000.0 / avg_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,};
char value[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, "var", variable, sizeof(variable)) == ESP_OK &&
httpd_query_key_value(buf, "val", value, sizeof(value)) == ESP_OK) {
} 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 val = atoi(value);
sensor_t * s = esp_camera_sensor_get();
int res = 0;
if (!strcmp(variable, "framesize")) {
if (s->pixformat == PIXFORMAT_JPEG) res = s->set_framesize(s, (framesize_t)val);
}
else if (!strcmp(variable, "quality")) res = s->set_quality(s, val);
else if (!strcmp(variable, "contrast")) res = s->set_contrast(s, val);
else if (!strcmp(variable, "brightness")) res = s->set_brightness(s, val);
else if (!strcmp(variable, "saturation")) res = s->set_saturation(s, val);
else if (!strcmp(variable, "gainceiling")) res = s->set_gainceiling(s, (gainceiling_t)val);
else if (!strcmp(variable, "colorbar")) res = s->set_colorbar(s, val);
else if (!strcmp(variable, "awb")) res = s->set_whitebal(s, val);
else if (!strcmp(variable, "agc")) res = s->set_gain_ctrl(s, val);
else if (!strcmp(variable, "aec")) res = s->set_exposure_ctrl(s, val);
else if (!strcmp(variable, "hmirror")) res = s->set_hmirror(s, val);
else if (!strcmp(variable, "vflip")) res = s->set_vflip(s, val);
else if (!strcmp(variable, "awb_gain")) res = s->set_awb_gain(s, val);
else if (!strcmp(variable, "agc_gain")) res = s->set_agc_gain(s, val);
else if (!strcmp(variable, "aec_value")) res = s->set_aec_value(s, val);
else if (!strcmp(variable, "aec2")) res = s->set_aec2(s, val);
else if (!strcmp(variable, "dcw")) res = s->set_dcw(s, val);
else if (!strcmp(variable, "bpc")) res = s->set_bpc(s, val);
else if (!strcmp(variable, "wpc")) res = s->set_wpc(s, val);
else if (!strcmp(variable, "raw_gma")) res = s->set_raw_gma(s, val);
else if (!strcmp(variable, "lenc")) res = s->set_lenc(s, val);
else if (!strcmp(variable, "special_effect")) res = s->set_special_effect(s, val);
else if (!strcmp(variable, "wb_mode")) res = s->set_wb_mode(s, val);
else if (!strcmp(variable, "ae_level")) res = s->set_ae_level(s, val);
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 status_handler(httpd_req_t *req) {
static char json_response[1024];
sensor_t * s = esp_camera_sensor_get();
char * p = json_response;
*p++ = '{';
p += sprintf(p, "\"framesize\":%u,", s->status.framesize);
p += sprintf(p, "\"quality\":%u,", s->status.quality);
p += sprintf(p, "\"brightness\":%d,", s->status.brightness);
p += sprintf(p, "\"contrast\":%d,", s->status.contrast);
p += sprintf(p, "\"saturation\":%d,", s->status.saturation);
p += sprintf(p, "\"special_effect\":%u,", s->status.special_effect);
p += sprintf(p, "\"wb_mode\":%u,", s->status.wb_mode);
p += sprintf(p, "\"awb\":%u,", s->status.awb);
p += sprintf(p, "\"awb_gain\":%u,", s->status.awb_gain);
p += sprintf(p, "\"aec\":%u,", s->status.aec);
p += sprintf(p, "\"aec2\":%u,", s->status.aec2);
p += sprintf(p, "\"ae_level\":%d,", s->status.ae_level);
p += sprintf(p, "\"aec_value\":%u,", s->status.aec_value);
p += sprintf(p, "\"agc\":%u,", s->status.agc);
p += sprintf(p, "\"agc_gain\":%u,", s->status.agc_gain);
p += sprintf(p, "\"gainceiling\":%u,", s->status.gainceiling);
p += sprintf(p, "\"bpc\":%u,", s->status.bpc);
p += sprintf(p, "\"wpc\":%u,", s->status.wpc);
p += sprintf(p, "\"raw_gma\":%u,", s->status.raw_gma);
p += sprintf(p, "\"lenc\":%u,", s->status.lenc);
p += sprintf(p, "\"hmirror\":%u,", s->status.hmirror);
p += sprintf(p, "\"dcw\":%u,", s->status.dcw);
p += sprintf(p, "\"colorbar\":%u", s->status.colorbar);
*p++ = '}';
*p++ = 0;
httpd_resp_set_type(req, "application/json");
httpd_resp_set_hdr(req, "Access-Control-Allow-Origin", "*");
return httpd_resp_send(req, json_response, strlen(json_response));
}
static esp_err_t index_handler(httpd_req_t *req) {
httpd_resp_set_type(req, "text/html");
String page = "";
page += "<meta name=\"viewport\" content=\"width=device-width\">\n";
page += "<p><h2></h2></p><br/><br/><br/>";
page += "<script>var xhttp = new XMLHttpRequest();</script>";
page += "<script>function getsend(arg) { xhttp.open('GET', arg +'?' + new Date().getTime(), true); xhttp.send() } </script>";
//page += "<p align=center><IMG SRC='http://" + WiFiAddr + ":81/stream' style='width:280px;'></p><br/><br/>";
page += "<p align=center><IMG SRC='http://" + WiFiAddr + ":81/stream' style='width:600px; transform:rotate(0deg);'></p><br/>";
//page += "<p align=center> <button style=width:90px;height:80px onmousedown=getsend('go') onmouseup=getsend('stop') ontouchstart=getsend('go') ontouchend=getsend('stop') ></button> </p>";
//page += "<p align=center>";
//page += "<button style=width:90px;height:80px onmousedown=getsend('left') onmouseup=getsend('stop') ontouchstart=getsend('left') ontouchend=getsend('stop')></button> ";
//page += "<button style=width:90px;height:80px onmousedown=getsend('stop') onmouseup=getsend('stop')></button> ";
//page += "<button style=width:90px;height:80px onmousedown=getsend('right') onmouseup=getsend('stop') ontouchstart=getsend('right') ontouchend=getsend('stop')></button>";
//page += "</p>";
//page += "<p align=center><button style=width:90px;height:80px onmousedown=getsend('back') onmouseup=getsend('stop') ontouchstart=getsend('back') ontouchend=getsend('stop') ></button></p>";
page += "<p align=center><br/><br/>";
page += "<button style=width:120px;height:40px;background:#768d87 onmousedown=getsend('links')>links</button> ";
page += "<button style=width:150px;height:40px;background:#91b8b3 onmousedown=getsend('reboot')>ESP Restart</button> ";
page += "<button style=width:120px;height:40px;background:#768d87 onmousedown=getsend('rechts')>rechts</button>";
page += "</p>";
return httpd_resp_send(req, &page[0], strlen(&page[0]));
}
static esp_err_t reboot_handler(httpd_req_t *req) {
delay(500);
Serial.println("rebooting");
delay(2000);
ESP.restart();
httpd_resp_set_type(req, "text/html");
return httpd_resp_send(req, "OK", 2);
}
static esp_err_t links_handler(httpd_req_t *req) {
Servo myservo;
if (posDegrees >= 0 && posDegrees <= 180) {
posDegrees = posDegrees + posDegreesStep;
}
if (posDegrees > 180) {
posDegrees = 180;
}
else {
myservo.write(posDegrees); // move the servo to calculated angle
Serial.print("Moved to: ");
Serial.print(posDegrees); // print the angle
Serial.println(" degree");
}
httpd_resp_set_type(req, "text/html");
return httpd_resp_send(req, "OK", 2);
}
static esp_err_t rechts_handler(httpd_req_t *req) {
Servo myservo;
if (posDegrees > 0 && posDegrees <= 180) {
posDegrees = posDegrees - posDegreesStep;
}
if (posDegrees < 0) {
posDegrees = 0;
} else {
myservo.write(posDegrees); // move the servo to calculated angle
Serial.print("Moved to: ");
Serial.print(posDegrees); // print the angle
Serial.println(" degree");
}
httpd_resp_set_type(req, "text/html");
return httpd_resp_send(req, "OK", 2);
}
void startCameraServer() {
httpd_config_t config = HTTPD_DEFAULT_CONFIG();
httpd_uri_t reboot_uri = {
.uri = "/reboot",
.method = HTTP_GET,
.handler = reboot_handler,
.user_ctx = NULL
};
httpd_uri_t links_uri = {
.uri = "/links",
.method = HTTP_GET,
.handler = links_handler,
.user_ctx = NULL
};
httpd_uri_t rechts_uri = {
.uri = "/rechts",
.method = HTTP_GET,
.handler = rechts_handler,
.user_ctx = NULL
};
httpd_uri_t index_uri = {
.uri = "/",
.method = HTTP_GET,
.handler = index_handler,
.user_ctx = NULL
};
httpd_uri_t status_uri = {
.uri = "/status",
.method = HTTP_GET,
.handler = status_handler,
.user_ctx = NULL
};
httpd_uri_t cmd_uri = {
.uri = "/control",
.method = HTTP_GET,
.handler = cmd_handler,
.user_ctx = NULL
};
httpd_uri_t capture_uri = {
.uri = "/capture",
.method = HTTP_GET,
.handler = capture_handler,
.user_ctx = NULL
};
httpd_uri_t stream_uri = {
.uri = "/stream",
.method = HTTP_GET,
.handler = stream_handler,
.user_ctx = NULL
};
ra_filter_init(&ra_filter, 20);
Serial.printf("Starting web server on port: '%d'", config.server_port);
if (httpd_start(&camera_httpd, &config) == ESP_OK) {
httpd_register_uri_handler(camera_httpd, &index_uri);
httpd_register_uri_handler(camera_httpd, &reboot_uri);
httpd_register_uri_handler(camera_httpd, &links_uri);
httpd_register_uri_handler(camera_httpd, &rechts_uri);
} else {
Serial.printf("Starting web server failed");
}
config.server_port += 1;
config.ctrl_port += 1;
Serial.printf("Starting stream server on port: '%d'", config.server_port);
if (httpd_start(&stream_httpd, &config) == ESP_OK) {
httpd_register_uri_handler(stream_httpd, &stream_uri);
}
}

Answer 1

I had the exact same issue. It's probably because the timers for the camera are interfering with the servo timers. You have to use this library, ServoESP32, and attach your servo like this:
Servo remote;
remote.attach(16, 2);
All this does is attach your servo to channel 2 given that channels 0 and 1 are being used by the CAM. I tried this and it fixed the problem instantly.