diff --git a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp
index e9e6b5fc..45c2a524 100644
--- a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp
+++ b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp
@@ -57,15 +57,9 @@ ESP32SvelteKit::ESP32SvelteKit(PsychicHttpServer *server, unsigned int numberEnd
       _factoryResetService(server, &ESPFS, &_securitySettingsService),
       _systemStatus(server, &_securitySettingsService),
       _fileExplorer(server, &_securitySettingsService),
-#if FT_ENABLED(FT_SERVO)
-      _servoController(server, &ESPFS, &_securitySettingsService, &_socket),
-#endif
+      _servoController(server, &ESPFS, &_securitySettingsService, &_peripherals, &_socket),
 #if FT_ENABLED(FT_MOTION)
-      _motionService(_server, &_socket, &_securitySettingsService,
-#if FT_ENABLED(FT_SERVO)
-                     &_servoController,
-#endif
-                     &_taskManager),
+      _motionService(_server, &_socket, &_securitySettingsService, &_servoController, &_taskManager),
 #endif
 #if FT_ENABLED(FT_WS2812)
       _ledService(&_taskManager),
@@ -197,9 +191,7 @@ void ESP32SvelteKit::startServices() {
     _cameraSettingsService.begin();
 #endif
     _peripherals.begin();
-#if FT_ENABLED(FT_SERVO)
     _servoController.begin();
-#endif
 #if FT_ENABLED(FT_WS2812)
     _ledService.begin();
 #endif
@@ -215,9 +207,7 @@ void IRAM_ATTR ESP32SvelteKit::loop() {
 #if FT_ENABLED(FT_BATTERY)
         _batteryService.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 2f61dbd3..3259e27a 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/MotionService.h b/esp32/lib/ESP32-sveltekit/MotionService.h
index 616d2089..3ddb00c7 100644
--- a/esp32/lib/ESP32-sveltekit/MotionService.h
+++ b/esp32/lib/ESP32-sveltekit/MotionService.h
@@ -20,16 +20,12 @@ enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, WALK };
 class MotionService {
   public:
     MotionService(PsychicHttpServer *server, EventSocket *socket, SecurityManager *securityManager,
-#if FT_ENABLED(FT_SERVO)
-                  ServoController *servoController,
-#endif
-                  TaskManager *taskManager)
+                  ServoController *servoController, TaskManager *taskManager)
         : _server(server),
           _socket(socket),
           _securityManager(securityManager),
           _servoController(servoController),
-          _taskManager(taskManager) {
-    }
+          _taskManager(taskManager) {}
 
     void begin() {
         _socket->onEvent(INPUT_EVENT, [&](JsonObject &root, int originId) { handleInput(root, originId); });
@@ -89,8 +85,8 @@ class MotionService {
     }
 
     void handleMode(JsonObject &root, int originId) {
-        ESP_LOGV("MotionService", "Mode %d", root["data"].as<int>());
         motionState = (MOTION_STATE)root["data"].as<int>();
+        ESP_LOGV("MotionService", "Mode %d", motionState);
         char output[2];
         itoa((int)motionState, output, 10);
         motionState == MOTION_STATE::DEACTIVATED ? _servoController->deactivate() : _servoController->activate();
@@ -103,6 +99,7 @@ class MotionService {
                  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_EVENT, output, originId.c_str());
+        _servoController->setAngles(angles);
     }
 
     bool updateMotion() {
@@ -120,7 +117,7 @@ class MotionService {
     bool update_angles(float new_angles[12], float angles[12], bool useLerp = true) {
         bool updated = false;
         for (int i = 0; i < 12; i++) {
-            float new_angle = useLerp ? lerp(angles[i], new_angles[i] * dir[i], 0.3) : new_angles[i] * dir[i];
+            float new_angle = useLerp ? lerp(angles[i], new_angles[i], 0.3) : new_angles[i];
             if (!isEqual(new_angle, angles[i], 0.1)) {
                 angles[i] = new_angle;
                 updated = true;
@@ -154,7 +151,6 @@ class MotionService {
     body_state_t body_state = {0, 0, 0, 0, 0, 0};
     float new_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
 
-    float dir[12] = {-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1};
     float default_feet_positions[4][4] = {
         {100, -100, 100, 1}, {100, -100, -100, 1}, {-100, -100, 100, 1}, {-100, -100, -100, 1}};
 
diff --git a/esp32/lib/ESP32-sveltekit/Peripherals.h b/esp32/lib/ESP32-sveltekit/Peripherals.h
index 14ae3c16..7bd6a6a7 100644
--- a/esp32/lib/ESP32-sveltekit/Peripherals.h
+++ b/esp32/lib/ESP32-sveltekit/Peripherals.h
@@ -241,8 +241,16 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
     }
 
     /* SERVO FUNCTIONS*/
+    void pcaLerpTo(int index, int value, float t) {
+#if FT_ENABLED(FT_SERVO)
+        target_pwm[index] = lerp(pwm[index], value, t);
+#endif
+    }
+
     void pcaWrite(int index, int value) {
 #if FT_ENABLED(FT_SERVO)
+        pwm[index] = value;
+        target_pwm[index] = value;
         _pca.setPWM(index, 0, value);
 #endif
     }
@@ -444,8 +452,8 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 #if FT_ENABLED(FT_SERVO)
     Adafruit_PWMServoDriver _pca;
     bool _pca_active {false};
-    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};
+    uint16_t pwm[16] = {0};
+    uint16_t target_pwm[16] = {0};
 #endif
 #if FT_ENABLED(FT_IMU)
     MPU6050 _imu;
diff --git a/esp32/lib/ESP32-sveltekit/ServoController.h b/esp32/lib/ESP32-sveltekit/ServoController.h
index 80815c71..3f8f741f 100644
--- a/esp32/lib/ESP32-sveltekit/ServoController.h
+++ b/esp32/lib/ESP32-sveltekit/ServoController.h
@@ -14,8 +14,9 @@
 
 class ServoController {
   public:
-    ServoController(PsychicHttpServer *server, FS *fs, SecurityManager *securityManager, EventSocket *socket)
-        : _server(server), _securityManager(securityManager), _socket(socket) {}
+    ServoController(PsychicHttpServer *server, FS *fs, SecurityManager *securityManager, Peripherals *peripherals,
+                    EventSocket *socket)
+        : _server(server), _securityManager(securityManager), _peripherals(peripherals), _socket(socket) {}
 
     void begin() {
         _socket->onEvent(EVENT_SERVO_CONFIGURATION_SETTINGS,
@@ -38,23 +39,15 @@ class ServoController {
         _socket->emit("angles", output, String(originId).c_str());
     }
 
-    void deactivate() {
-        if (!is_active) return;
-        is_active = false;
-        pca.sleep();
-    }
+    void deactivate() { _peripherals->pcaDeactivate(); }
 
-    void activate() {
-        if (is_active) return;
-        is_active = true;
-        pca.wakeup();
-    }
+    void activate() { _peripherals->pcaActivate(); }
 
     void updateActiveState() { is_active ? activate() : deactivate(); }
 
     void setAngles(float angles[12]) {
         for (int i = 0; i < 12; i++) {
-            this->target_angles[i] = angles[i] * dir[i];
+            this->target_angles[i] = angles[i];
         }
     }
 
@@ -62,8 +55,8 @@ class ServoController {
         for (int i = 0; i < 12; i++) {
             this->angles[i] = lerp(this->angles[i], target_angles[i], 0.2);
             int16_t angle = dir[i] * angles[i] + 90;
-            uint16_t pwm = angle * servo_conversion[i] + min_pwm[i];
-            pca.setPWM(i, 0, pwm);
+            uint16_t pwm = min(angle * servo_conversion[i] + min_pwm[i], 4095.f);
+            _peripherals->pcaWrite(i, pwm);
         }
     }
 
@@ -74,12 +67,12 @@ class ServoController {
   private:
     PsychicHttpServer *_server;
     SecurityManager *_securityManager;
+    Peripherals *_peripherals;
     EventSocket *_socket;
 
-    Adafruit_PWMServoDriver pca;
     bool is_active {true};
     constexpr static int ServoInterval = 2;
-    int8_t dir[12] = {-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1};
+    int8_t dir[12] = {-1, 1, 1, -1, -1, -1, 1, 1, 1, 1, -1, -1};
     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};
     float min_pwm[12] = {125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125};