diff --git a/include/OV2640.h b/include/OV2640.h new file mode 100644 index 0000000..39927df --- /dev/null +++ b/include/OV2640.h @@ -0,0 +1,43 @@ +#ifndef OV2640_H_ +#define OV2640_H_ + +#include +#include +#include +#include "esp_log.h" +#include "esp_attr.h" +#include "esp_camera.h" + +extern camera_config_t esp32cam_config, esp32cam_aithinker_config, esp32cam_ttgo_t_config; + +class OV2640 +{ +public: + OV2640(){ + fb = NULL; + }; + ~OV2640(){ + }; + esp_err_t init(camera_config_t config); + void run(void); + size_t getSize(void); + uint8_t *getfb(void); + int getWidth(void); + int getHeight(void); + framesize_t getFrameSize(void); + pixformat_t getPixelFormat(void); + + void setFrameSize(framesize_t size); + void setPixelFormat(pixformat_t format); + +private: + void runIfNeeded(); // grab a frame if we don't already have one + + // camera_framesize_t _frame_size; + // camera_pixelformat_t _pixel_format; + camera_config_t _cam_config; + + camera_fb_t *fb; +}; + +#endif //OV2640_H_ \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 9c9f08b..4286e9e 100644 --- a/platformio.ini +++ b/platformio.ini @@ -14,9 +14,9 @@ board = esp32cam monitor_speed = 115200 framework = arduino lib_deps = - ;ottowinter/ESPAsyncWebServer-esphome@^2.1.0 teckel12/NewPing@^1.9.7 rfetick/MPU6050_light@^1.1.0 adafruit/Adafruit PWM Servo Driver Library@^2.4.1 adafruit/Adafruit SSD1306@^2.5.7 - adafruit/Adafruit GFX Library@^1.11.5 \ No newline at end of file + adafruit/Adafruit GFX Library@^1.11.5 + links2004/WebSockets@^2.4.1 diff --git a/src/OV2640.cpp b/src/OV2640.cpp new file mode 100644 index 0000000..174fa52 --- /dev/null +++ b/src/OV2640.cpp @@ -0,0 +1,193 @@ +#include "OV2640.h" + +#define TAG "OV2640" + +// definitions appropriate for the ESP32-CAM devboard (and most clones) +camera_config_t esp32cam_config{ + + .pin_pwdn = -1, // FIXME: on the TTGO T-Journal I think this is GPIO 0 + .pin_reset = 15, + + .pin_xclk = 27, + + .pin_sscb_sda = 25, + .pin_sscb_scl = 23, + + .pin_d7 = 19, + .pin_d6 = 36, + .pin_d5 = 18, + .pin_d4 = 39, + .pin_d3 = 5, + .pin_d2 = 34, + .pin_d1 = 35, + .pin_d0 = 17, + .pin_vsync = 22, + .pin_href = 26, + .pin_pclk = 21, + .xclk_freq_hz = 20000000, + .ledc_timer = LEDC_TIMER_0, + .ledc_channel = LEDC_CHANNEL_0, + .pixel_format = PIXFORMAT_JPEG, + // .frame_size = FRAMESIZE_UXGA, // needs 234K of framebuffer space + // .frame_size = FRAMESIZE_SXGA, // needs 160K for framebuffer + // .frame_size = FRAMESIZE_XGA, // needs 96K or even smaller FRAMESIZE_SVGA - can work if using only 1 fb + .frame_size = FRAMESIZE_SVGA, + .jpeg_quality = 12, //0-63 lower numbers are higher quality + .fb_count = 2 // if more than one i2s runs in continous mode. Use only with jpeg +}; + +camera_config_t esp32cam_aithinker_config{ + + .pin_pwdn = 32, + .pin_reset = -1, + + .pin_xclk = 0, + + .pin_sscb_sda = 26, + .pin_sscb_scl = 27, + + // Note: LED GPIO is apparently 4 not sure where that goes + // per https://github.com/donny681/ESP32_CAMERA_QR/blob/e4ef44549876457cd841f33a0892c82a71f35358/main/led.c + .pin_d7 = 35, + .pin_d6 = 34, + .pin_d5 = 39, + .pin_d4 = 36, + .pin_d3 = 21, + .pin_d2 = 19, + .pin_d1 = 18, + .pin_d0 = 5, + .pin_vsync = 25, + .pin_href = 23, + .pin_pclk = 22, + .xclk_freq_hz = 20000000, + .ledc_timer = LEDC_TIMER_1, + .ledc_channel = LEDC_CHANNEL_1, + .pixel_format = PIXFORMAT_JPEG, + // .frame_size = FRAMESIZE_UXGA, // needs 234K of framebuffer space + // .frame_size = FRAMESIZE_SXGA, // needs 160K for framebuffer + // .frame_size = FRAMESIZE_XGA, // needs 96K or even smaller FRAMESIZE_SVGA - can work if using only 1 fb + .frame_size = FRAMESIZE_SVGA, + .jpeg_quality = 12, //0-63 lower numbers are higher quality + .fb_count = 2 // if more than one i2s runs in continous mode. Use only with jpeg +}; + +camera_config_t esp32cam_ttgo_t_config{ + + .pin_pwdn = 26, + .pin_reset = -1, + + .pin_xclk = 32, + + .pin_sscb_sda = 13, + .pin_sscb_scl = 12, + + .pin_d7 = 39, + .pin_d6 = 36, + .pin_d5 = 23, + .pin_d4 = 18, + .pin_d3 = 15, + .pin_d2 = 4, + .pin_d1 = 14, + .pin_d0 = 5, + .pin_vsync = 27, + .pin_href = 25, + .pin_pclk = 19, + .xclk_freq_hz = 20000000, + .ledc_timer = LEDC_TIMER_0, + .ledc_channel = LEDC_CHANNEL_0, + .pixel_format = PIXFORMAT_JPEG, + .frame_size = FRAMESIZE_SVGA, + .jpeg_quality = 12, //0-63 lower numbers are higher quality + .fb_count = 2 // if more than one i2s runs in continous mode. Use only with jpeg +}; + +void OV2640::run(void) +{ + if (fb) + //return the frame buffer back to the driver for reuse + esp_camera_fb_return(fb); + + fb = esp_camera_fb_get(); +} + +void OV2640::runIfNeeded(void) +{ + if (!fb) + run(); +} + +int OV2640::getWidth(void) +{ + runIfNeeded(); + return fb->width; +} + +int OV2640::getHeight(void) +{ + runIfNeeded(); + return fb->height; +} + +size_t OV2640::getSize(void) +{ + runIfNeeded(); + if (!fb) + return 0; // FIXME - this shouldn't be possible but apparently the new cam board returns null sometimes? + return fb->len; +} + +uint8_t *OV2640::getfb(void) +{ + runIfNeeded(); + if (!fb) + return NULL; // FIXME - this shouldn't be possible but apparently the new cam board returns null sometimes? + + return fb->buf; +} + +framesize_t OV2640::getFrameSize(void) +{ + return _cam_config.frame_size; +} + +void OV2640::setFrameSize(framesize_t size) +{ + _cam_config.frame_size = size; +} + +pixformat_t OV2640::getPixelFormat(void) +{ + return _cam_config.pixel_format; +} + +void OV2640::setPixelFormat(pixformat_t format) +{ + switch (format) + { + case PIXFORMAT_RGB565: + case PIXFORMAT_YUV422: + case PIXFORMAT_GRAYSCALE: + case PIXFORMAT_JPEG: + _cam_config.pixel_format = format; + break; + default: + _cam_config.pixel_format = PIXFORMAT_GRAYSCALE; + break; + } +} + +esp_err_t OV2640::init(camera_config_t config) +{ + memset(&_cam_config, 0, sizeof(_cam_config)); + memcpy(&_cam_config, &config, sizeof(config)); + + esp_err_t err = esp_camera_init(&_cam_config); + if (err != ESP_OK) + { + printf("Camera probe failed with error 0x%x", err); + return err; + } + // ESP_ERROR_CHECK(gpio_install_isr_service(0)); + + return ESP_OK; +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index fdf14ba..961bfce 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,200 +1,345 @@ -#include "esp_camera.h" +#include +#include "OV2640.h" #include -#include "esp_timer.h" -#include "img_converters.h" -#include "Arduino.h" -#include "fb_gfx.h" -#include "soc/soc.h" -#include "soc/rtc_cntl_reg.h" -#include "esp_http_server.h" +#include +#include +#include + +//#define USE_SONAR +//#define USE_MPU +//#define USE_OLED +//#define USE_PWM +#define USE_WEBSOCKET +//#define USE_BUTTON + +#define FILESYSTEM SPIFFS +#if FILESYSTEM == FFat +#include +#endif +#if FILESYSTEM == SPIFFS +#include +#endif + +#ifdef USE_SONAR #include -#include +#endif +//#include #include "Wire.h" +#ifdef USE_OLED #include #include +#endif +#ifdef USE_MPU #include +#endif +#ifdef USE_PWM #include +#endif +#ifdef USE_WEBSOCKET +#include +#endif + #define CAMERA_MODEL_AI_THINKER #include -const char* ssid = "SSID"; -const char* password = "PASSWORD"; +const char* ssid = ""; +const char* password = ""; -#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"; - -#define SCREEN_WIDTH 128 -#define SCREEN_HEIGHT 64 +const char HEADER[] = "HTTP/1.1 200 OK\r\n" \ + "Access-Control-Allow-Origin: *\r\n" \ + "Content-Type: multipart/x-mixed-replace; boundary=123456789000000000000987654321\r\n"; +const char BOUNDARY[] = "\r\n--123456789000000000000987654321\r\n"; +const char CTNTTYPE[] = "Content-Type: image/jpeg\r\nContent-Length: "; +const int hdrLen = strlen(HEADER); +const int bdrLen = strlen(BOUNDARY); +const int cntLen = strlen(CTNTTYPE); +#ifdef USE_BUTTON int buttonLed = 2; int button = 16; +bool servosEnabled = true; +int currentButtonState = 0; +#endif +#ifdef USE_SONAR NewPing sonar[2] = { NewPing(12, 12, 200), NewPing(13, 13, 200) }; +#endif -MPU6050 mpu(Wire); -Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40); -Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1); +#ifdef USE_MPU + MPU6050 mpu(Wire); + bool MPU_READY = false; +#endif -httpd_handle_t stream_httpd = NULL; +#ifdef USE_PWM + Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40); + bool PWM_READY = false; + +#endif +#ifdef USE_OLED + #define SCREEN_WIDTH 128 + #define SCREEN_HEIGHT 64 + Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1); + bool OLED_READY = false; +#endif + +#ifdef USE_WEBSOCKET +WebSocketsServer webSocket = WebSocketsServer(81); +#endif +OV2640 cam; + +WebServer server(80); long timer = 0; + +String formatBytes(size_t bytes) { + if (bytes < 1024) { + return String(bytes) + "B"; + } else if (bytes < (1024 * 1024)) { + return String(bytes / 1024.0) + "KB"; + } else if (bytes < (1024 * 1024 * 1024)) { + return String(bytes / 1024.0 / 1024.0) + "MB"; + } else { + return String(bytes / 1024.0 / 1024.0 / 1024.0) + "GB"; + } +} + +String getContentType(String filename) { + if (server.hasArg("download")) { + return "application/octet-stream"; + } else if (filename.endsWith(".htm")) { + return "text/html"; + } else if (filename.endsWith(".html")) { + return "text/html"; + } else if (filename.endsWith(".css")) { + return "text/css"; + } else if (filename.endsWith(".js")) { + return "application/javascript"; + } else if (filename.endsWith(".png")) { + return "image/png"; + } else if (filename.endsWith(".gif")) { + return "image/gif"; + } else if (filename.endsWith(".jpg")) { + return "image/jpeg"; + } else if (filename.endsWith(".ico")) { + return "image/x-icon"; + } else if (filename.endsWith(".xml")) { + return "text/xml"; + } else if (filename.endsWith(".pdf")) { + return "application/x-pdf"; + } else if (filename.endsWith(".zip")) { + return "application/x-zip"; + } else if (filename.endsWith(".gz")) { + return "application/x-gzip"; + } + return "text/plain"; +} + +bool exists(String path){ + bool yes = false; + File file = FILESYSTEM.open(path, "r"); + if(!file.isDirectory()){ + yes = true; + } + file.close(); + return yes; +} + +bool loadFromSdCard(String path) { + Serial.println("handleFileRead: " + path); + if (path.endsWith("/")) { + path += "index.htm"; + } + String contentType = getContentType(path); + String pathWithGz = path + ".gz"; + if (exists(pathWithGz) || exists(path)) { + if (exists(pathWithGz)) { + path += ".gz"; + } + File file = FILESYSTEM.open(path, "r"); + Serial.println(file); + server.streamFile(file, contentType); + file.close(); + return true; + } + return false; +} + +#ifdef USE_WEBSOCKET +void webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t length) { + Serial.printf("Got event [%u]", type); + switch(type) { + case WStype_DISCONNECTED: + Serial.printf("[%u] Disconnected!\n", num); + break; + case WStype_CONNECTED: + { + IPAddress ip = webSocket.remoteIP(num); + Serial.printf("[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload); + + // send message to client + webSocket.sendTXT(num, "Connected"); + } + break; + case WStype_TEXT: + Serial.printf("[%u] get Text: %s\n", num, payload); + + // send message to client + // webSocket.sendTXT(num, "message here"); + + // send data to all connected clients + // webSocket.broadcastTXT("message here"); + break; + case WStype_BIN: + Serial.printf("[%u] get binary length: %u\n", num, length); + //hexdump(payload, length); + + // send message to client + // webSocket.sendBIN(num, payload, length); + break; + case WStype_ERROR: + case WStype_FRAGMENT_TEXT_START: + case WStype_FRAGMENT_BIN_START: + case WStype_FRAGMENT: + case WStype_FRAGMENT_FIN: + break; + } + +} +#endif + +#ifdef USE_BUTTON static unsigned long last_interrupt_time = 0; -bool servosEnabled = true; +#endif +void handle_jpg_stream(void) +{ + char buf[32]; + int s; -void toggleServo(){ - Serial.print("Servos is enabled:"); - Serial.println(servosEnabled); - //if(servosEnabled){ - // pwm.sleep(); - //} else { - // pwm.wakeup(); - //} - servosEnabled = !servosEnabled; -} + WiFiClient client = server.client(); -void IRAM_ATTR powerToggle(){ - unsigned long interrupt_time = millis(); - if (interrupt_time - last_interrupt_time > 200) { - toggleServo(); - digitalWrite(buttonLed, servosEnabled); - } - last_interrupt_time = interrupt_time; -} + client.write(HEADER, hdrLen); + client.write(BOUNDARY, bdrLen); -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]; - - 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.println("Camera capture failed"); - res = ESP_FAIL; - } else { - if(fb->width > 400){ - 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.println("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; - } - Serial.printf("MJPG: %uB\n",(uint32_t)(_jpg_buf_len)); - } - return res; -} - -void startCameraServer(){ - httpd_config_t config = HTTPD_DEFAULT_CONFIG(); - config.server_port = 80; - - httpd_uri_t index_uri = { - .uri = "/", - .method = HTTP_GET, - .handler = stream_handler, - .user_ctx = NULL - }; - - Serial.printf("Starting web server on port: '%d'\n", config.server_port); - if (httpd_start(&stream_httpd, &config) == ESP_OK) { - httpd_register_uri_handler(stream_httpd, &index_uri); + while (true) + { + if (!client.connected()) break; + cam.run(); + s = cam.getSize(); + client.write(CTNTTYPE, cntLen); + sprintf( buf, "%d\r\n\r\n", s ); + client.write(buf, strlen(buf)); + client.write((char *)cam.getfb(), s); + client.write(BOUNDARY, bdrLen); } } -void setupMPU(){ - Wire.begin(14, 15); - +const char JHEADER[] = "HTTP/1.1 200 OK\r\n" \ + "Content-disposition: inline; filename=capture.jpg\r\n" \ + "Content-type: image/jpeg\r\n\r\n"; +const int jhdLen = strlen(JHEADER); + +void handle_jpg(void) +{ + WiFiClient client = server.client(); + + if (!client.connected()) return; + cam.run(); + client.write(JHEADER, jhdLen); + client.write((char *)cam.getfb(), cam.getSize()); +} + +void handle_stream_viewing() +{ + char temp[index_simple_html_len]; + + snprintf(temp, index_simple_html_len, index_simple_html); + server.send(200, "text/html; charset=utf-8", index_simple_html); +} + +void handleNotFound() +{ + if (loadFromSdCard(server.uri())) { + Serial.println("Sending file from SPIFFS"); + return; + } + String message = "Server is running!\n\n"; + message += "URI: "; + message += server.uri(); + message += "\nMethod: "; + message += (server.method() == HTTP_GET) ? "GET" : "POST"; + message += "\nArguments: "; + message += server.args(); + message += "\n"; + server.send(200, "text/plain", message); +} +#ifdef USE_MPU +bool setupMPU(){ byte status = mpu.begin(); Serial.print(F("MPU6050 status: ")); Serial.println(status); - while(status!=0){ - status = mpu.begin(); - Serial.print(F("MPU6050 status: ")); - Serial.println(status); - delay(500); - } // stop everything if could not connect to MPU6050 - + if(status != 0){ return 0; } Serial.println(F("Calculating offsets, do not move MPU6050")); delay(1000); - mpu.calcOffsets(true,true); // gyro and accelero + mpu.calcOffsets(true,true); Serial.println("Done!\n"); + return 1; } - -void setupOLED(){ +#endif +#ifdef USE_OLED +bool setupOLED(){ if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { Serial.println(F("SSD1306 allocation failed")); - for(;;); // Don't proceed, loop forever + return 0; } display.display(); delay(200); display.clearDisplay(); + return 1; } - -void setupPWMController(){ +#endif +#ifdef USE_PWM +bool setupPWMController(){ pwm.begin(); pwm.setOscillatorFrequency(27000000); pwm.setPWMFreq(50); + return 1; } - +#endif +#ifdef USE_BUTTON +bool debounce() { + static uint16_t state = 0; + state = (state<<1) | digitalRead(button) | 0xfe00; + return (state == 0xff00); +} +#endif void setup() { - WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); - + //WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); + Serial.begin(115200); Serial.setDebugOutput(false); + Wire.begin(14, 15); + + #ifdef USE_BUTTON pinMode(buttonLed, OUTPUT); pinMode(button, INPUT); - - attachInterrupt(button, powerToggle, RISING); + #endif - setupMPU(); - setupOLED(); - setupPWMController(); + #ifdef USE_MPU + MPU_READY = setupMPU(); + #endif + #ifdef USE_PWM + PWM_READY = setupPWMController(); + #endif + #ifdef USE_OLED + OLED_READY = setupOLED(); + #endif camera_config_t config; config.ledc_channel = LEDC_CHANNEL_0; @@ -229,11 +374,7 @@ void setup() { } // 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; - } + cam.init(config); // Wi-Fi connection WiFi.begin(ssid, password); while (WiFi.status() != WL_CONNECTED) { @@ -245,44 +386,45 @@ void setup() { Serial.print("Camera Stream Ready! Go to: http://"); Serial.print(WiFi.localIP()); - display.setTextSize(2); // Normal 1:1 pixel scale - display.setTextColor(WHITE); // Draw white text - display.setCursor(0,0); // Start at top-left corner - display.println(WiFi.localIP()); - display.display(); + #ifdef USE_OLED + if(OLED_READY){ + display.setTextSize(2); + display.setTextColor(WHITE); + display.setCursor(0,0); + display.println(WiFi.localIP()); + display.display(); + } + #endif - startCameraServer(); + server.on("/mjpeg/1", HTTP_GET, handle_jpg_stream); + server.on("/jpg", HTTP_GET, handle_jpg); + server.on("/", HTTP_GET, handle_stream_viewing); + server.onNotFound(handleNotFound); + server.begin(); + + webSocket.begin(); + webSocket.onEvent(webSocketEvent); } void loop() { - delay(1); - /*for (uint8_t i = 0; i < 2; i++) { // Loop through each sensor and display results. - delay(200); // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings. - Serial.print(i); - Serial.print("="); - Serial.print(sonar[i].ping_cm()); - Serial.print("cm "); - } - mpu.update(); + server.handleClient(); + webSocket.loop(); - if(millis() - timer > 1000){ // print data every second - Serial.print(F("TEMPERATURE: "));Serial.println(mpu.getTemp()); - Serial.print(F("ACCELERO X: "));Serial.print(mpu.getAccX()); - Serial.print("\tY: ");Serial.print(mpu.getAccY()); - Serial.print("\tZ: ");Serial.println(mpu.getAccZ()); - - Serial.print(F("GYRO X: "));Serial.print(mpu.getGyroX()); - Serial.print("\tY: ");Serial.print(mpu.getGyroY()); - Serial.print("\tZ: ");Serial.println(mpu.getGyroZ()); - - Serial.print(F("ACC ANGLE X: "));Serial.print(mpu.getAccAngleX()); - Serial.print("\tY: ");Serial.println(mpu.getAccAngleY()); - - Serial.print(F("ANGLE X: "));Serial.print(mpu.getAngleX()); - Serial.print("\tY: ");Serial.print(mpu.getAngleY()); - Serial.print("\tZ: ");Serial.println(mpu.getAngleZ()); - Serial.println(F("=====================================================\n")); + + if(millis() - timer > 500) { + Serial.println("Sending message to websocket client"); + String letme = "LET ME TELL YOU SOMETHING"; + if(webSocket.broadcastTXT(letme)){ + Serial.println("Send message to websocket client"); + } timer = millis(); - }*/ + } + + #ifdef USE_BUTTON + if (debounce()) { + servosEnabled = !servosEnabled; + digitalWrite(buttonLed, servosEnabled); + } + #endif }