🏎️ Updates servo controller to reflect rl angles

app/src/routes/peripherals/servo/servo.svelte

@@ -13,7 +13,7 @@
 <div>
     <h2 class="text-lg">{ servo.name }</h2>
     <div class="flex gap-2 items-center">
-        Is inverted <input type="checkbox" checked={servo.inverted} class="checkbox"/>
+        Is inverted <input type="checkbox" bind:checked={servo.inverted} class="toggle"/>
     </div>
     <div>
         Middle position <input type="number" bind:value={servo.center_angle} class="input input-bordered input-sm max-w-xs"/>

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 isLoading = false;
-	let servos: Servo[];
-	let last_servo_config: ServoConfiguration;
 
-	let isLoading = true;
+    let active = false
 
-	$: updateServoConfig(servo_config, servos);
+    let servoId = 0
 
-	const updateServoConfig = async (servo_config: ServoConfiguration, servos: Servo[]) => {
+    const throttler = new Throttler()
-		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 sweep = (event:any) => {
         let channel = event.detail.channel;
         socket.sendEvent('servoConfiguration', {servos:[{channel, sweep: true}]});
     };
 
-	onMount(() => {
+    const activateServo = (event:any) => {
-		socket.on('servoConfiguration', (data: ServoConfiguration) => {
+        socket.sendEvent('servoState', {'active':1});
-			isLoading = false;
+    };
-			servo_config = data;
+
-			servos = data.servos;
+    const deactivateServo = (event:any) => {
-			last_servo_config = structuredClone(data);
+        socket.sendEvent('servoState', {'active':0});
-		});
+    };
-	});
+
+    let pwm = 306;
+
+    const updatePWM = () => {
+        throttler.throttle(() => {
+            socket.sendEvent('servoPWM', {servo_id:servoId, pwm});
+        }, 10)
+    }
 </script>
 
 <SettingsCard collapsible={false}>
 	<MotorOutline slot="icon" class="lex-shrink-0 mr-2 h-6 w-6 self-end" />
 	<span slot="title">Servo</span>
+    <input type="range" min="200" max="400" bind:value={pwm} on:input={updatePWM} class="w-full h-2 bg-gray-200 rounded-lg appearance-none cursor-pointer dark:bg-gray-700">
 
 	{#if isLoading}
 		<Spinner />
 	{:else}
 		<div class="flex flex-col">
 			<h2 class="text-lg">General servo configuration</h2>
-			<span class="mb-1 flex justify-between items-center">
+            <span class="flex items-center gap-2">
-				Servo Oscillator Frequency <input
+                <label for="servoId">Servo active{servoId}</label>
-					type="number"
+                <input type="checkbox" class="toggle" bind:checked={active} on:change={active ? deactivateServo : activateServo}>
-					bind:value={servo_config.servo_oscillator_frequency}
+            </span>
-					class="input input-bordered input-sm max-w-xs"
+        </div>
-				/>
-			</span>
-			<span class="flex justify-between items-center mb-1">
-				Servo PWM Frequency <input
-					type="number"
-					bind:value={servo_config.servo_pwm_frequency}
-					class="input input-bordered input-sm max-w-xs"
-				/>
-			</span>
-			<span class="flex justify-between items-center gap-1">
-				Is active <input type="checkbox" bind:checked={servo_config.is_active} class="toggle" />
-			</span>
-		</div>
-		<div class="divider"></div>
-		{#each servos as servo}
-			<ServoController bind:servo on:sweep={sweep} />
-			<div class="divider"></div>
-		{/each}
 	{/if}
 </SettingsCard>

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'));
+    }
 </script>
 
 <div class="navbar bg-base-300 sticky top-0 z-10 h-12 min-h-fit drop-shadow-lg lg:h-16 gap-2">
@@ -40,6 +45,11 @@
 		>
 		<h1 class="px-2 text-xl font-bold lg:text-2xl">{$page.data.title}</h1>
 	</div>
+    <!-- svelte-ignore a11y-click-events-have-key-events -->
+    <!-- svelte-ignore a11y-no-static-element-interactions -->
+    <div class="indicator flex-none" on:click={deactivate}>
+		<Power class="h-7 w-7"/>
+	</div>
 	<div class="indicator flex-none">
 		<UpdateIndicator />
 	</div>

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

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

esp32/lib/ESP32-sveltekit/ServoController.h

@@ -1,211 +1,73 @@
+#ifndef ServoController_h
+#define ServoController_h
+
 #include <Adafruit_PWMServoDriver.h>
 #include <EventEndpoint.h>
 #include <FSPersistence.h>
 #include <HttpEndpoint.h>
 #include <SecurityManager.h>
 #include <StatefulService.h>
+#include <MathUtils.h>
+#include <Timing.h>
 
-#define SERVO_CONFIG_FILE "/config/servoConfig.json"
+#define EVENT_SERVO_CONFIGURATION_SETTINGS "servoPWM"
-#define SERVO_CONFIGURATION_SETTINGS_PATH "/api/servo/configuration"
+#define EVENT_SERVO_STATE "servoState"
-#define SERVO_EVENT_CONFIGURATION_SETTINGS "servoConfiguration"
 
-#ifndef FACTORY_SERVO_NUM
+class ServoController {
-#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 {
   public:
-    int32_t servo_oscillator_frequency {FACTORY_SERVO_OSCILLATOR_FREQUENCY};
+    ServoController(PsychicHttpServer *server, FS *fs, SecurityManager *securityManager, Peripherals *peripherals,
-    int32_t servo_pwm_frequency {FACTORY_SERVO_PWM_FREQUENCY};
+                    EventSocket *socket)
-    std::vector<servo_t> servos_config;
+        : _server(server), _securityManager(securityManager), _peripherals(peripherals), _socket(socket) {}
-    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) {
+    void begin() {
-        root["is_active"] = settings.is_active;
+        _socket->onEvent(EVENT_SERVO_CONFIGURATION_SETTINGS,
-        root["servo_pwm_frequency"] = settings.servo_pwm_frequency;
+                         [&](JsonObject &root, int originId) { servoEvent(root, originId); });
-        root["servo_oscillator_frequency"] = settings.servo_oscillator_frequency;
+        _socket->onEvent(EVENT_SERVO_STATE, [&](JsonObject &root, int originId) { stateUpdate(root, originId); });
+    }
 
-        JsonArray servos = root["servos"].to<JsonArray>();
+    void stateUpdate(JsonObject &root, int originId) {
+        bool active = root["active"].as<bool>();
+        ESP_LOGI("SERVOCONTROLLER", "Setting state %d", active);
+        active ? activate() : deactivate();
+    }
 
-        for (auto &servo : settings.servos_config) {
+    void servoEvent(JsonObject &root, int originId) {
-            JsonObject servo_config = servos.add<JsonObject>();
+        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;
+    void syncAngles(const String &originId) {
-            servo_config["channel"] = servo.channel;
+        char output[100];
-            servo_config["inverted"] = servo.inverted;
+        snprintf(output, sizeof(output), "[%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f]", angles[0],
-            servo_config["angle"] = servo.angle;
+                 angles[1], angles[2], angles[3], angles[4], angles[5], angles[6], angles[7], angles[8], angles[9],
-            servo_config["center_angle"] = servo.center_angle;
+                 angles[10], angles[11]);
-            servo_config["state"] = servo.state;
+        _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<JsonArray>() && 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<JsonObject>();
-            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<String>() || servo->name;
-                if (servo_config["inverted"]) servo->inverted = servo_config["inverted"];
-                if (servo_config["angle"].is<int16_t>()) {
-                    servo->angle = servo_config["angle"].as<int16_t>();
-                    servo->state = SERVO_STATE_ACTIVE;
-                }
-                if (servo_config["center_angle"].is<int16_t>())
-                    servo->center_angle = servo_config["center_angle"].as<int16_t>();
-                if (servo_config["sweep"]) servo->state = SERVO_STATE_SWEEPING_FORWARD;
-                continue;
-            }
-
-            servo_t new_servo;
-            new_servo.name = servo_config["name"].as<String>();
-            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<servo_t> &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<ServoConfiguration> {
-  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) {
+        for (int i = 0; i < 12; i++) {
-            if (servo.state == SERVO_STATE::SERVO_STATE_ACTIVE) {
+            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<S
   private:
     PsychicHttpServer *_server;
     SecurityManager *_securityManager;
-    EventEndpoint<ServoConfiguration> _eventEndpoint;
+    Peripherals *_peripherals;
-    FSPersistence<ServoConfiguration> _fsPersistence;
+    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