From 4e75952f576fce2207445a60d7e1d7ffcf07e130 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Sat, 3 Aug 2024 14:02:30 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=8F=8E=EF=B8=8F=20Updates=20servo=20contr?= =?UTF-8?q?oller=20to=20reflect=20rl=20angles?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/src/routes/peripherals/servo/servo.svelte | 2 +- .../routes/peripherals/servo/servos.svelte | 100 ++----- app/src/routes/statusbar.svelte | 10 + esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp | 14 +- esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h | 2 - esp32/lib/ESP32-sveltekit/ServoController.h | 255 +++++------------- 6 files changed, 106 insertions(+), 277 deletions(-) diff --git a/app/src/routes/peripherals/servo/servo.svelte b/app/src/routes/peripherals/servo/servo.svelte index c1d9cde..c340271 100644 --- a/app/src/routes/peripherals/servo/servo.svelte +++ b/app/src/routes/peripherals/servo/servo.svelte @@ -13,7 +13,7 @@

{ servo.name }

- Is inverted + Is inverted
Middle position diff --git a/app/src/routes/peripherals/servo/servos.svelte b/app/src/routes/peripherals/servo/servos.svelte index cce07c6..c04f002 100644 --- a/app/src/routes/peripherals/servo/servos.svelte +++ b/app/src/routes/peripherals/servo/servos.svelte @@ -3,100 +3,56 @@ import type { ServoConfiguration, Servo } from '$lib/models'; import MotorOutline from '~icons/mdi/motor-outline'; import ServoController from './servo.svelte'; - import { api } from '$lib/api'; import Spinner from '$lib/components/Spinner.svelte'; + import { socket } from '$lib/stores'; - import { onMount } from 'svelte'; + import { onDestroy, onMount } from 'svelte'; + import { throttler as Throttler } from '$lib/utilities'; - let servo_config: ServoConfiguration; - let servos: Servo[]; - let last_servo_config: ServoConfiguration; + let isLoading = false; - let isLoading = true; + let active = false - $: updateServoConfig(servo_config, servos); + let servoId = 0 - const updateServoConfig = async (servo_config: ServoConfiguration, servos: Servo[]) => { - if (!servo_config) return; - const changes: { [key: string]: any } = {}; - for (const key of Object.keys(servo_config)) { - if (key == 'servos') { - for (let i = 0; i < servo_config.servos.length; i++) { - for (const servo_key of Object.keys(servo_config.servos[i])) { - if ( - JSON.stringify(servo_config.servos[i][servo_key as keyof Servo]) !== - JSON.stringify(last_servo_config.servos[i][servo_key as keyof Servo]) - ) { - if (!changes.servos) changes.servos = []; - if (!changes.servos[i]) changes.servos[i] = {}; - changes.servos[i][servo_key as keyof Servo] = - servo_config.servos[i][servo_key as keyof Servo]; - changes.servos[i].channel = servo_config.servos[i].channel; - } - } - } - continue; - } - if ( - JSON.stringify(servo_config[key as keyof ServoConfiguration]) !== - JSON.stringify(last_servo_config[key as keyof ServoConfiguration]) - ) { - changes[key as keyof ServoConfiguration] = servo_config[key as keyof ServoConfiguration]; - } - } - - if (Object.keys(changes).length > 0) { - socket.sendEvent('servoConfiguration', changes); - last_servo_config = structuredClone(servo_config); - } - }; + const throttler = new Throttler() const sweep = (event:any) => { let channel = event.detail.channel; socket.sendEvent('servoConfiguration', {servos:[{channel, sweep: true}]}); }; - onMount(() => { - socket.on('servoConfiguration', (data: ServoConfiguration) => { - isLoading = false; - servo_config = data; - servos = data.servos; - last_servo_config = structuredClone(data); - }); - }); + const activateServo = (event:any) => { + socket.sendEvent('servoState', {'active':1}); + }; + + const deactivateServo = (event:any) => { + socket.sendEvent('servoState', {'active':0}); + }; + + let pwm = 306; + + const updatePWM = () => { + throttler.throttle(() => { + socket.sendEvent('servoPWM', {servo_id:servoId, pwm}); + }, 10) + } Servo + {#if isLoading} {:else}

General servo configuration

- - Servo Oscillator Frequency - - - Servo PWM Frequency - - - Is active - -
-
- {#each servos as servo} - -
- {/each} + + + + +
{/if} diff --git a/app/src/routes/statusbar.svelte b/app/src/routes/statusbar.svelte index df03418..de7cd8c 100644 --- a/app/src/routes/statusbar.svelte +++ b/app/src/routes/statusbar.svelte @@ -13,6 +13,7 @@ import MdiWeatherSunny from '~icons/mdi/weather-sunny'; import MdiMoonAndStars from '~icons/mdi/moon-and-stars'; import { api } from '$lib/api'; + import { mode, modes, socket } from '$lib/stores'; const postSleep = async () => await api.post('/api/sleep') @@ -30,6 +31,10 @@ } }); } + + const deactivate = async () => { + mode.set(modes.indexOf('deactivated')); + }
@@ -40,6 +45,11 @@ >

{$page.data.title}

+ + +
+ +
diff --git a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp index 7f586da..ed7c5e3 100644 --- a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp +++ b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp @@ -57,14 +57,12 @@ ESP32SvelteKit::ESP32SvelteKit(PsychicHttpServer *server, unsigned int numberEnd _factoryResetService(server, &ESPFS, &_securitySettingsService), _systemStatus(server, &_securitySettingsService), _fileExplorer(server, &_securitySettingsService), + _servoController(server, &ESPFS, &_securitySettingsService, &_peripherals, &_socket), #if FT_ENABLED(FT_MOTION) _motionService(_server, &_socket, &_securitySettingsService, &_taskManager), #endif #if FT_ENABLED(FT_WS2812) _ledService(&_taskManager), -#endif -#if FT_ENABLED(FT_SERVO) - _servoController(server, &ESPFS, &_securitySettingsService, &_socket), #endif _peripherals(server, &ESPFS, &_securitySettingsService, &_socket) { } @@ -186,17 +184,14 @@ void ESP32SvelteKit::startServices() { #endif _taskManager.begin(); _fileExplorer.begin(); + _peripherals.begin(); + _servoController.begin(); #if FT_ENABLED(FT_MOTION) _motionService.begin(); #endif #if FT_ENABLED(FT_CAMERA) _cameraService.begin(); _cameraSettingsService.begin(); -#endif - _peripherals.begin(); -#if FT_ENABLED(FT_SERVO) - _servoController.begin(); - _servoController.configure(); #endif #if FT_ENABLED(FT_WS2812) _ledService.begin(); @@ -216,9 +211,6 @@ void IRAM_ATTR ESP32SvelteKit::loop() { #if FT_ENABLED(FT_MOTION) _motionService.loop(); #endif -#if FT_ENABLED(FT_SERVO) - _servoController.loop(); -#endif #if FT_ENABLED(FT_WS2812) _ledService.loop(); #endif diff --git a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h index 80b5038..97eac37 100644 --- a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h +++ b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h @@ -176,9 +176,7 @@ class ESP32SvelteKit { CameraSettingsService _cameraSettingsService; #endif Peripherals _peripherals; -#if FT_ENABLED(FT_SERVO) ServoController _servoController; -#endif #if FT_ENABLED(FT_WS2812) LEDService _ledService; #endif diff --git a/esp32/lib/ESP32-sveltekit/ServoController.h b/esp32/lib/ESP32-sveltekit/ServoController.h index 7224815..52cefd5 100644 --- a/esp32/lib/ESP32-sveltekit/ServoController.h +++ b/esp32/lib/ESP32-sveltekit/ServoController.h @@ -1,211 +1,73 @@ +#ifndef ServoController_h +#define ServoController_h + #include #include #include #include #include #include +#include +#include -#define SERVO_CONFIG_FILE "/config/servoConfig.json" -#define SERVO_CONFIGURATION_SETTINGS_PATH "/api/servo/configuration" -#define SERVO_EVENT_CONFIGURATION_SETTINGS "servoConfiguration" +#define EVENT_SERVO_CONFIGURATION_SETTINGS "servoPWM" +#define EVENT_SERVO_STATE "servoState" -#ifndef FACTORY_SERVO_NUM -#define FACTORY_SERVO_NUM 12 -#endif - -#ifndef FACTORY_SERVO_PWM_FREQUENCY -#define FACTORY_SERVO_PWM_FREQUENCY 50 -#endif - -#ifndef FACTORY_SERVO_OSCILLATOR_FREQUENCY -#define FACTORY_SERVO_OSCILLATOR_FREQUENCY 27000000 -#endif - -#ifndef FACTORY_SERVO_CENTER_ANGLE -#define FACTORY_SERVO_CENTER_ANGLE 90 -#endif - -#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; - SERVO_STATE state; -}; - -class ServoConfiguration { +class ServoController { public: - 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 {true}; - const int8_t servo_invert[12] = {-1, 1, 1, -1, -1, -1, 1, 1, 1, 1, -1, -1}; + ServoController(PsychicHttpServer *server, FS *fs, SecurityManager *securityManager, Peripherals *peripherals, + EventSocket *socket) + : _server(server), _securityManager(securityManager), _peripherals(peripherals), _socket(socket) {} - 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; + void begin() { + _socket->onEvent(EVENT_SERVO_CONFIGURATION_SETTINGS, + [&](JsonObject &root, int originId) { servoEvent(root, originId); }); + _socket->onEvent(EVENT_SERVO_STATE, [&](JsonObject &root, int originId) { stateUpdate(root, originId); }); + } - JsonArray servos = root["servos"].to(); + void stateUpdate(JsonObject &root, int originId) { + bool active = root["active"].as(); + ESP_LOGI("SERVOCONTROLLER", "Setting state %d", active); + active ? activate() : deactivate(); + } - for (auto &servo : settings.servos_config) { - JsonObject servo_config = servos.add(); + void servoEvent(JsonObject &root, int originId) { + uint8_t servo_id = root["servo_id"]; + uint16_t pwm = root["pwm"]; + center[servo_id] = pwm; + ESP_LOGI("SERVO_CONTROLLER", "Setting servo %d to %d", servo_id, pwm); + } - servo_config["name"] = servo.name; - servo_config["channel"] = servo.channel; - servo_config["inverted"] = servo.inverted; - servo_config["angle"] = servo.angle; - servo_config["center_angle"] = servo.center_angle; - servo_config["state"] = servo.state; + void syncAngles(const String &originId) { + char output[100]; + snprintf(output, sizeof(output), "[%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f]", angles[0], + angles[1], angles[2], angles[3], angles[4], angles[5], angles[6], angles[7], angles[8], angles[9], + angles[10], angles[11]); + _socket->emit("angles", output, String(originId).c_str()); + } + + void deactivate() { _peripherals->pcaDeactivate(); } + + void activate() { _peripherals->pcaActivate(); } + + void updateActiveState() { is_active ? activate() : deactivate(); } + + void setAngles(float new_angles[12]) { + for (int i = 0; i < 12; i++) { + target_angles[i] = new_angles[i]; } } - 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() && 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("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 { - public: - ServoController(PsychicHttpServer *server, FS *fs, SecurityManager *securityManager, EventSocket *socket) - : Adafruit_PWMServoDriver(), - _server(server), - _securityManager(securityManager), - _eventEndpoint(ServoConfiguration::read, ServoConfiguration::update, this, socket, - SERVO_EVENT_CONFIGURATION_SETTINGS), - _fsPersistence(ServoConfiguration::read, ServoConfiguration::update, this, fs, SERVO_CONFIG_FILE) { - addUpdateHandler([&](const String &originId) { updateServos(); }, false); - } - - void configure() { - _eventEndpoint.begin(); - _fsPersistence.readFromFS(); - setOscillatorFrequency(_state.servo_oscillator_frequency); - setPWMFreq(_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; - 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) { + for (int i = 0; i < 12; i++) { + angles[i] = lerp(angles[i], target_angles[i], 0.2); + float angle = dir[i] * angles[i] + center_angle_deg[i]; + uint16_t pwm = angle * servo_conversion[i] + center[i]; + if (pwm < 125 || pwm > 600) { + ESP_LOGE("ServoController", "Servo %d, Invalid PWM value %d", i, pwm); continue; - } else if (servo.state == SERVO_STATE::SERVO_STATE_SWEEPING_FORWARD) { - servo.angle += 2; - if (servo.angle >= 180) { - servo.state = SERVO_STATE::SERVO_STATE_SWEEPING_BACKWARD; - } - } else if (servo.state == SERVO_STATE::SERVO_STATE_SWEEPING_BACKWARD) { - servo.angle -= 2; - if (servo.angle <= 0) { - servo.state = SERVO_STATE::SERVO_STATE_ACTIVE; - } } - setAngle(&servo); + _peripherals->pcaWrite(i, pwm); } } @@ -216,9 +78,20 @@ class ServoController : public Adafruit_PWMServoDriver, public StatefulService _eventEndpoint; - FSPersistence _fsPersistence; + Peripherals *_peripherals; + EventSocket *_socket; bool is_active {true}; constexpr static int ServoInterval = 2; + + float center[12] = {306, 306, 306, 306, 306, 306, 306, 306, 306, 306, 306, 306}; + float dir[12] = {-1, 1, 1, -1, -1, -1, 1, 1, 1, 1, -1, -1}; + float center_angle_deg[12] = {0, -45, 90, 0, 45, -90, 0, -45, 90, 0, 45, -90}; + + float angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145}; + float target_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145}; + const float servo_conversion[12] {2.2, 2.1055555, 1.96923, 2.2, 2.1055555, 1.96923, + 2.2, 2.1055555, 1.96923, 2.2, 2.1055555, 1.96923}; }; + +#endif \ No newline at end of file