From 813dde318c7e56a1144e9b7029b0a03b72c6bcdc Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Mon, 10 Jun 2024 21:16:53 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=8D=A1=20Adds=20sweeping=20for=20servo?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/src/lib/stores/socket.ts | 10 +- app/src/routes/peripherals/servo/servo.svelte | 9 +- .../routes/peripherals/servo/servos.svelte | 135 +++++++---- esp32/features.ini | 2 + esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp | 3 + esp32/lib/ESP32-sveltekit/ServoController.h | 227 +++++++++++++----- esp32/platformio.ini | 2 +- 7 files changed, 279 insertions(+), 109 deletions(-) diff --git a/app/src/lib/stores/socket.ts b/app/src/lib/stores/socket.ts index e230c25..889b0d1 100644 --- a/app/src/lib/stores/socket.ts +++ b/app/src/lib/stores/socket.ts @@ -1,10 +1,12 @@ import { writable } from 'svelte/store'; +const socketEvents = ['open', 'close', 'error', 'message', 'unresponsive'] as const; +type SocketEvent = (typeof socketEvents)[number]; + function createWebSocket() { let listeners = new Map void>>(); const { subscribe, set } = writable(false); - const socketEvents = ['open', 'close', 'error', 'message', 'unresponsive'] as const; - type SocketEvent = (typeof socketEvents)[number]; + const reconnectTimeoutTime = 5000; let unresponsiveTimeoutId: number; let reconnectTimeoutId: number; let ws: WebSocket; @@ -21,7 +23,7 @@ function createWebSocket() { clearTimeout(unresponsiveTimeoutId); clearTimeout(reconnectTimeoutId); listeners.get(reason)?.forEach((listener) => listener(event)); - reconnectTimeoutId = setTimeout(connect, 1000); + reconnectTimeoutId = setTimeout(connect, reconnectTimeoutTime); } function connect() { @@ -74,7 +76,7 @@ function createWebSocket() { function resetUnresponsiveCheck() { clearTimeout(unresponsiveTimeoutId); - unresponsiveTimeoutId = setTimeout(() => disconnect('unresponsive'), 2000); + unresponsiveTimeoutId = setTimeout(() => disconnect('unresponsive'), reconnectTimeoutTime); } function send(msg: unknown) { diff --git a/app/src/routes/peripherals/servo/servo.svelte b/app/src/routes/peripherals/servo/servo.svelte index 2956472..c1d9cde 100644 --- a/app/src/routes/peripherals/servo/servo.svelte +++ b/app/src/routes/peripherals/servo/servo.svelte @@ -1,6 +1,13 @@
@@ -19,5 +26,5 @@ 90 180
- + \ No newline at end of file diff --git a/app/src/routes/peripherals/servo/servos.svelte b/app/src/routes/peripherals/servo/servos.svelte index 350072b..cce07c6 100644 --- a/app/src/routes/peripherals/servo/servos.svelte +++ b/app/src/routes/peripherals/servo/servos.svelte @@ -1,51 +1,102 @@ - - Servo + + Servo - {#await getServoConfig() } - - {:then _} -
-

General servo configuration

- - Servo Oscillator Frequency - - - Servo PWM Frequency - - - Is active - -
-
- {#each servo_config.servos as servo} - -
- {/each} - {/await} - \ No newline at end of file + {#if isLoading} + + {:else} +
+

General servo configuration

+ + Servo Oscillator Frequency + + + Servo PWM Frequency + + + Is active + +
+
+ {#each servos as servo} + +
+ {/each} + {/if} + diff --git a/esp32/features.ini b/esp32/features.ini index 38f6a88..c504e70 100644 --- a/esp32/features.ini +++ b/esp32/features.ini @@ -9,6 +9,8 @@ build_flags = -D FT_DOWNLOAD_FIRMWARE=0 -D FT_ANALYTICS=1 -D FT_MOTION=0 + + ; Hardware specific -D FT_IMU=0 -D FT_MAG=0 -D FT_BMP=0 diff --git a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp index 602ce7c..9052bd1 100644 --- a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp +++ b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp @@ -240,6 +240,9 @@ void IRAM_ATTR ESP32SvelteKit::_loop() { #endif #if FT_ENABLED(FT_MOTION) _motionService.loop(); +#endif +#if FT_ENABLED(FT_SERVO) + _servoController.loop(); #endif vTaskDelay(20 / portTICK_PERIOD_MS); } diff --git a/esp32/lib/ESP32-sveltekit/ServoController.h b/esp32/lib/ESP32-sveltekit/ServoController.h index 5b86db4..1885868 100644 --- a/esp32/lib/ESP32-sveltekit/ServoController.h +++ b/esp32/lib/ESP32-sveltekit/ServoController.h @@ -1,55 +1,62 @@ +#include #include #include #include #include #include -#include - #define SERVO_CONFIG_FILE "/config/servoConfig.json" #define SERVO_CONFIGURATION_SETTINGS_PATH "/api/servo/configuration" +#define EVENT_CONFIGURATION_SETTINGS "servoConfiguration" #ifndef FACTORY_SERVO_NUM - #define FACTORY_SERVO_NUM 12 +#define FACTORY_SERVO_NUM 12 #endif #ifndef FACTORY_SERVO_PWM_FREQUENCY - #define FACTORY_SERVO_PWM_FREQUENCY 50 +#define FACTORY_SERVO_PWM_FREQUENCY 50 #endif #ifndef FACTORY_SERVO_OSCILLATOR_FREQUENCY - #define FACTORY_SERVO_OSCILLATOR_FREQUENCY 27000000 +#define FACTORY_SERVO_OSCILLATOR_FREQUENCY 27000000 #endif #ifndef FACTORY_SERVO_CENTER_ANGLE - #define FACTORY_SERVO_CENTER_ANGLE 90 +#define FACTORY_SERVO_CENTER_ANGLE 90 #endif -struct servo_t -{ +#define SERVO_STATE_SPEED_MS 20 +#define SERVO_MIN 150 // This is the 'minimum' pulse length count (out of 4096) +#define SERVO_MAX 650 // This is the 'maximum' pulse length count (out of 4096) + +enum SERVO_STATE { SERVO_STATE_ACTIVE, SERVO_STATE_SWEEPING_FORWARD, SERVO_STATE_SWEEPING_BACKWARD }; + +struct servo_t { String name; int8_t channel; - bool inverted; - int16_t angle; - int16_t center_angle; + bool inverted; + int16_t angle; + int16_t center_angle; + SERVO_STATE state; }; class ServoConfiguration { public: - int32_t servo_oscillator_frequency {FACTORY_SERVO_OSCILLATOR_FREQUENCY}; - int32_t servo_pwm_frequency {FACTORY_SERVO_PWM_FREQUENCY}; + int32_t servo_oscillator_frequency{FACTORY_SERVO_OSCILLATOR_FREQUENCY}; + int32_t servo_pwm_frequency{FACTORY_SERVO_PWM_FREQUENCY}; std::vector servos_config; - bool is_active {false}; + bool is_active{true}; + const int8_t servo_invert[12] = {-1, 1, 1, -1, -1, -1, 1, 1, 1, 1, -1, -1}; static void read(ServoConfiguration &settings, JsonObject &root) { root["is_active"] = settings.is_active; root["servo_pwm_frequency"] = settings.servo_pwm_frequency; - root["servo_oscillator_frequency"] = settings.servo_oscillator_frequency; + root["servo_oscillator_frequency"] = + settings.servo_oscillator_frequency; JsonArray servos = root["servos"].to(); - for (auto &servo : settings.servos_config) - { + for (auto &servo : settings.servos_config) { JsonObject servo_config = servos.add(); servo_config["name"] = servo.name; @@ -57,84 +64,182 @@ class ServoConfiguration { servo_config["inverted"] = servo.inverted; servo_config["angle"] = servo.angle; servo_config["center_angle"] = servo.center_angle; + servo_config["state"] = servo.state; } } - static StateUpdateResult update(JsonObject &root, ServoConfiguration &settings) { - settings.is_active = root["is_active"] | false; - settings.servo_pwm_frequency = root["servo_pwm_frequency"] | FACTORY_SERVO_PWM_FREQUENCY; - settings.servo_oscillator_frequency = root["servo_oscillator_frequency"] | FACTORY_SERVO_OSCILLATOR_FREQUENCY; - settings.servos_config.clear(); + static StateUpdateResult update(JsonObject &root, + ServoConfiguration &settings) { + settings.is_active = root["is_active"] | settings.is_active; + settings.servo_pwm_frequency = + root["servo_pwm_frequency"] | settings.servo_pwm_frequency; + settings.servo_oscillator_frequency = + root["servo_oscillator_frequency"] | + settings.servo_oscillator_frequency; JsonArray servos = root["servos"]; - if (root["servos"].is()) - { - int i = 0; - for (auto servo : servos) - { - JsonObject servo_config = servo.as(); - servo_t new_servo; - new_servo.name = servo_config["name"].as(); - new_servo.channel = servo_config["channel"]; - new_servo.inverted = servo_config["inverted"]; - new_servo.angle = servo_config["angle"]; - new_servo.center_angle = servo_config["center_angle"]; - - settings.servos_config.push_back(new_servo); - i++; - } - } else { + if (!root["servos"].is() && settings.servos_config.empty()) { + ESP_LOGI("ControllerSettings", "No servos found, adding default servos"); for (int8_t i = 0; i < FACTORY_SERVO_NUM; i++) { - ESP_LOGI("WiFiSettings", "Adding servo %d", i); - settings.servos_config.push_back(servo_t { - .name = "Servo " + String(i), - .channel = i, - .inverted = 1, - .angle = 0, - .center_angle = FACTORY_SERVO_CENTER_ANGLE - }); + ESP_LOGI("ControllerSettings", "Adding servo %d", i); + settings.servos_config.push_back( + servo_t{.name = "Servo " + String(i), + .channel = i, + .inverted = 1, + .angle = 0, + .center_angle = FACTORY_SERVO_CENTER_ANGLE + // , + // .state = SERVO_STATE_ACTIVE + }); } + return StateUpdateResult::CHANGED; } + + for (auto new_servo_json : servos) { + JsonObject servo_config = new_servo_json.as(); + int8_t channel = servo_config["channel"] | -1; + servo_t *servo = + get_servo_by_channel(settings.servos_config, channel); + + if (servo != nullptr) { + servo->name = servo_config["name"].as() || servo->name; + if (servo_config["inverted"]) + servo->inverted = servo_config["inverted"]; + if (servo_config["angle"].is()) { + servo->angle = servo_config["angle"].as(); + servo->state = SERVO_STATE_ACTIVE; + } + if (servo_config["center_angle"].is()) + servo->center_angle = + servo_config["center_angle"].as(); + if (servo_config["sweep"]) + servo->state = SERVO_STATE_SWEEPING_FORWARD; + continue; + } + + servo_t new_servo; + new_servo.name = servo_config["name"].as(); + new_servo.channel = channel; + new_servo.inverted = servo_config["inverted"]; + new_servo.angle = servo_config["angle"]; + new_servo.center_angle = servo_config["center_angle"]; + // new_servo.state = servo_config["state"]; + + settings.servos_config.push_back(new_servo); + } + return StateUpdateResult::CHANGED; }; + static servo_t *get_servo_by_channel(std::vector &servos_config, + int8_t channel_id) { + for (auto &servo : servos_config) { + if (servo.channel == channel_id) { + return &servo; + } + } + return nullptr; + } }; -class ServoController : public Adafruit_PWMServoDriver, public StatefulService { +class ServoController : public Adafruit_PWMServoDriver, + public StatefulService { public: - ServoController(PsychicHttpServer *server, FS *fs, SecurityManager *securityManager, EventSocket *socket) - : Adafruit_PWMServoDriver(), _server(server), + ServoController(PsychicHttpServer *server, FS *fs, + SecurityManager *securityManager, EventSocket *socket) + : Adafruit_PWMServoDriver(), + _server(server), _securityManager(securityManager), - _httpEndpoint(ServoConfiguration::read, ServoConfiguration::update, - this, server, SERVO_CONFIGURATION_SETTINGS_PATH, - securityManager, AuthenticationPredicates::IS_ADMIN), - // _eventEndpoint(ServoConfiguration::read, ServoConfiguration::update, - // this, socket, EVENT_CONFIGURATION_SETTINGS), - _fsPersistence(ServoConfiguration::read, ServoConfiguration::update, this, fs, SERVO_CONFIG_FILE) { + _eventEndpoint(ServoConfiguration::read, ServoConfiguration::update, + this, socket, EVENT_CONFIGURATION_SETTINGS), + _fsPersistence(ServoConfiguration::read, ServoConfiguration::update, + this, fs, SERVO_CONFIG_FILE) { + addUpdateHandler([&](const String &originId) { updateServos(); }, + false); } void configure() { - _httpEndpoint.begin(); + _eventEndpoint.begin(); _fsPersistence.readFromFS(); setOscillatorFrequency(_state.servo_oscillator_frequency); setPWMFreq(_state.servo_pwm_frequency); - ESP_LOGI("ServoController", "Configured with oscillator frequency %d and PWM frequency %d", _state.servo_oscillator_frequency, _state.servo_pwm_frequency); + deactivate(); + ESP_LOGI("ServoController", + "Configured with oscillator frequency %d and PWM frequency %d", + _state.servo_oscillator_frequency, _state.servo_pwm_frequency); } void deactivate() { + if (!is_active) return; _state.is_active = false; + is_active = false; sleep(); } void activate() { + if (is_active) return; _state.is_active = true; - sleep(); + is_active = true; + wakeup(); + } + + void updateActiveState() { _state.is_active ? activate() : deactivate(); } + + void updateServos() { + updateActiveState(); + + if (!is_active) return; + + for (auto &servo : _state.servos_config) { + setAngle(&servo); + } + } + + void setAngle(servo_t* servo) { + int8_t channel = servo->channel; + bool invert = servo->inverted; + int16_t angle = invert ? 180 - servo->angle : servo->angle; + ESP_LOGV("ServoController", "Setting servo %d to angle %d", channel, angle); + setPWM(channel, 0, angleToPwm(angle)); + } + + uint16_t angleToPwm(int angle) { + return map(angle, 0, 180, SERVO_MIN, SERVO_MAX); + } + + void updateServoState() { + for (auto &servo : _state.servos_config) { + if (servo.state == SERVO_STATE::SERVO_STATE_ACTIVE) { + continue; + } else if (servo.state == SERVO_STATE::SERVO_STATE_SWEEPING_FORWARD) { + servo.angle += 1; + if (servo.angle >= 180) { + servo.state = SERVO_STATE::SERVO_STATE_SWEEPING_BACKWARD; + } + } else if (servo.state == SERVO_STATE::SERVO_STATE_SWEEPING_BACKWARD) { + servo.angle -= 1; + if (servo.angle <= 0) { + servo.state = SERVO_STATE::SERVO_STATE_ACTIVE; + } + } + setAngle(&servo); + } + } + + void loop() { + if (int currentMillis = millis(); !_lastUpdate || (currentMillis - _lastUpdate) >= ServoInterval) { + _lastUpdate = currentMillis; + updateServoState(); + } } private: PsychicHttpServer *_server; SecurityManager *_securityManager; - HttpEndpoint _httpEndpoint; - // EventEndpoint _eventEndpoint; + EventEndpoint _eventEndpoint; FSPersistence _fsPersistence; + + bool is_active{true}; + unsigned long _lastUpdate; + constexpr static int ServoInterval = 100; }; diff --git a/esp32/platformio.ini b/esp32/platformio.ini index 3610e24..fb89598 100644 --- a/esp32/platformio.ini +++ b/esp32/platformio.ini @@ -69,7 +69,7 @@ build_flags = ${factory_settings.build_flags} ${features.build_flags} ${build_settings.build_flags} - -D CORE_DEBUG_LEVEL=3 + -D CORE_DEBUG_LEVEL=4 -D register= -std=gnu++17 build_unflags = -std=gnu++11