diff --git a/esp32/features.ini b/esp32/features.ini
index c476c13f..e54a3d53 100644
--- a/esp32/features.ini
+++ b/esp32/features.ini
@@ -17,3 +17,4 @@ build_flags =
   -D FT_WS2812=1
   -D FT_SERVO=1
   -D FT_ADS1115=1
+  -D FT_USS=0
diff --git a/esp32/lib/ESP32-sveltekit/MotionService.h b/esp32/lib/ESP32-sveltekit/MotionService.h
index ff268835..616d2089 100644
--- a/esp32/lib/ESP32-sveltekit/MotionService.h
+++ b/esp32/lib/ESP32-sveltekit/MotionService.h
@@ -4,6 +4,7 @@
 #include <EventSocket.h>
 #include <TaskManager.h>
 #include <Kinematics.h>
+#include <ServoController.h>
 #include <Timing.h>
 #include <MathUtils.h>
 
@@ -26,9 +27,7 @@ class MotionService {
         : _server(server),
           _socket(socket),
           _securityManager(securityManager),
-#if FT_ENABLED(FT_SERVO)
           _servoController(servoController),
-#endif
           _taskManager(taskManager) {
     }
 
@@ -46,7 +45,7 @@ class MotionService {
 
         body_state.updateFeet(default_feet_positions);
 
-        _taskManager->createTask(this->_loopImpl, "MotionService", 4096, this, (tskIDLE_PRIORITY + 2));
+        _taskManager->createTask(this->_loopImpl, "MotionService", 4096, this, 3);
     }
 
     void anglesEvent(JsonObject &root, int originId) {
@@ -94,9 +93,7 @@ class MotionService {
         motionState = (MOTION_STATE)root["data"].as<int>();
         char output[2];
         itoa((int)motionState, output, 10);
-#if FT_ENABLED(FT_SERVO)
         motionState == MOTION_STATE::DEACTIVATED ? _servoController->deactivate() : _servoController->activate();
-#endif
         _socket->emit(MODE_EVENT, output, String(originId).c_str());
     }
 
@@ -114,10 +111,7 @@ class MotionService {
             case MOTION_STATE::IDLE: return false;
             case MOTION_STATE::CALIBRATION: update_angles(calibration_angles, new_angles, false); break;
             case MOTION_STATE::REST: update_angles(rest_angles, new_angles, false); break;
-            case MOTION_STATE::STAND: {
-                kinematics.calculate_inverse_kinematics(body_state, new_angles);
-                break;
-            }
+            case MOTION_STATE::STAND: kinematics.calculate_inverse_kinematics(body_state, new_angles); break;
             case MOTION_STATE::WALK: kinematics.calculate_inverse_kinematics(body_state, new_angles); break;
         }
         return update_angles(new_angles, angles);
@@ -150,9 +144,7 @@ class MotionService {
     EventSocket *_socket;
     SecurityManager *_securityManager;
     TaskManager *_taskManager;
-#if FT_ENABLED(FT_SERVO)
     ServoController *_servoController;
-#endif
     Kinematics kinematics;
 
     MOTION_STATE motionState = MOTION_STATE::DEACTIVATED;
diff --git a/esp32/lib/ESP32-sveltekit/Peripherals.h b/esp32/lib/ESP32-sveltekit/Peripherals.h
index 95f18fcb..14ae3c16 100644
--- a/esp32/lib/ESP32-sveltekit/Peripherals.h
+++ b/esp32/lib/ESP32-sveltekit/Peripherals.h
@@ -19,6 +19,7 @@
 #include <Adafruit_HMC5883_U.h>
 #include <Adafruit_Sensor.h>
 #include <Adafruit_ADS1X15.h>
+#include <NewPing.h>
 
 #define DEVICE_CONFIG_FILE "/config/peripheral.json"
 #define EVENT_CONFIGURATION_SETTINGS "peripheralSettings"
@@ -29,6 +30,7 @@
 #define I2C_INTERVAL 500
 #define MAX_ESP_IMU_SIZE 500
 #define EVENT_IMU "imu"
+#define EVENT_SERVO_STATE "servoState"
 
 /*
  * Servo Settings
@@ -61,6 +63,11 @@
 #define I2C_FREQUENCY 100000UL
 #endif
 
+/*
+ * Ultrasonic Sensor Settings
+ */
+#define MAX_DISTANCE 200
+
 class PinConfig {
   public:
     int pin;
@@ -134,6 +141,11 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
         _pca.begin();
         _pca.setPWMFreq(FACTORY_SERVO_PWM_FREQUENCY);
         _pca.setOscillatorFrequency(FACTORY_SERVO_OSCILLATOR_FREQUENCY);
+        _pca.sleep();
+        _socket->onEvent(EVENT_SERVO_STATE, [&](JsonObject &root, int originId) {
+            _pca_active = root["active"] | false;
+            _pca_active ? pcaActivate() : pcaDeactivate();
+        });
 #endif
 
 #if FT_ENABLED(FT_IMU)
@@ -167,10 +179,23 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
         }
         _ads.startADCReading(ADS1X15_REG_CONFIG_MUX_DIFF_0_1, /*continuous=*/false);
 #endif
+
+#if FT_ENABLED(FT_USS)
+        _left_sonar = new NewPing(USS_LEFT_PIN, USS_LEFT_PIN, MAX_DISTANCE);
+        _right_sonar = new NewPing(USS_RIGHT_PIN, USS_RIGHT_PIN, MAX_DISTANCE);
+#endif
     };
 
     void loop() {
-        EXECUTE_EVERY_N_MS(_updateInterval, updateImu());
+        unsigned long currentMillis = millis();
+
+        if (currentMillis - _lastIMUUpdate >= _updateInterval) {
+            _lastIMUUpdate = currentMillis;
+            readIMU();
+            updateImu();
+            readSonar();
+            emitSonar();
+        }
     }
 
     void updatePins() {
@@ -222,10 +247,33 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 #endif
     }
 
-    void pcaAngle(int index, int value) {
+    void pcaWriteAngle(int index, int angle) {
 #if FT_ENABLED(FT_SERVO)
-        // uint16_t pwm = SERVO_MIN + value * SERVO_ANGLE_PWM_RATIO;
-        _pca.setPWM(index, 0, 125 + value * 2);
+        _pca.setPWM(index, 0, 125 + angle * 2);
+#endif
+    }
+
+    void pcaWriteAngles(float *angles, int numServos, int offset = 0) {
+#if FT_ENABLED(FT_SERVO)
+        for (int i = 0; i < numServos; i++) {
+            pcaWriteAngle(i + offset, angles[i]);
+        }
+#endif
+    }
+
+    void pcaActivate() {
+#if FT_ENABLED(FT_SERVO)
+        if (_pca_active) return;
+        _pca_active = true;
+        _pca.wakeup();
+#endif
+    }
+
+    void pcaDeactivate() {
+#if FT_ENABLED(FT_SERVO)
+        if (!_pca_active) return;
+        _pca_active = false;
+        _pca.sleep();
 #endif
     }
 
@@ -362,6 +410,26 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
         }
     }
 
+    void readSonar() {
+#if FT_ENABLED(FT_USS)
+        _left_distance = _left_sonar->ping_cm();
+        delay(50);
+        _right_distance = _right_sonar->ping_cm();
+#endif
+    }
+
+    void emitSonar() {
+#if FT_ENABLED(FT_USS)
+
+        char output[16];
+        snprintf(output, sizeof(output), "[%.1f,%.1f]", _left_distance, _right_distance);
+        _socket->emit("sonar", output);
+#endif
+    }
+
+    float leftDistance() { return _left_distance; }
+    float rightDistance() { return _right_distance; }
+
   private:
     PsychicHttpServer *_server;
     EventSocket *_socket;
@@ -375,6 +443,9 @@ 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};
 #endif
 #if FT_ENABLED(FT_IMU)
     MPU6050 _imu;
@@ -400,9 +471,16 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 #if FT_ENABLED(FT_ADS1115)
     Adafruit_ADS1115 _ads;
 #endif
+#if FT_ENABLED(FT_USS)
+    NewPing *_left_sonar;
+    NewPing *_right_sonar;
+#endif
+    float _left_distance {MAX_DISTANCE};
+    float _right_distance {MAX_DISTANCE};
 
     std::list<uint8_t> addressList;
     bool i2c_active = false;
+    unsigned long _lastIMUUpdate {0};
     unsigned long _updateInterval {I2C_INTERVAL};
 };
 
diff --git a/esp32/lib/ESP32-sveltekit/ServoController.h b/esp32/lib/ESP32-sveltekit/ServoController.h
index b16679b4..80815c71 100644
--- a/esp32/lib/ESP32-sveltekit/ServoController.h
+++ b/esp32/lib/ESP32-sveltekit/ServoController.h
@@ -1,3 +1,6 @@
+#ifndef ServoController_h
+#define ServoController_h
+
 #include <Adafruit_PWMServoDriver.h>
 #include <EventEndpoint.h>
 #include <FSPersistence.h>
@@ -6,37 +9,17 @@
 #include <StatefulService.h>
 #include <MathUtils.h>
 
-#define EVENT_CONFIGURATION_SETTINGS "servoConfiguration"
+#define EVENT_SERVO_CONFIGURATION_SETTINGS "servoConfiguration"
 #define EVENT_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
-
 class ServoController {
   public:
     ServoController(PsychicHttpServer *server, FS *fs, SecurityManager *securityManager, EventSocket *socket)
         : _server(server), _securityManager(securityManager), _socket(socket) {}
 
     void begin() {
-        pca.begin();
-        pca.setOscillatorFrequency(FACTORY_SERVO_OSCILLATOR_FREQUENCY);
-        pca.setPWMFreq(FACTORY_SERVO_PWM_FREQUENCY);
-        deactivate();
-        ESP_LOGI("ServoController", "Configured with oscillator frequency %d and PWM frequency %d",
-                 FACTORY_SERVO_OSCILLATOR_FREQUENCY, FACTORY_SERVO_PWM_FREQUENCY);
-
-        _socket->onEvent(EVENT_CONFIGURATION_SETTINGS,
+        _socket->onEvent(EVENT_SERVO_CONFIGURATION_SETTINGS,
                          [&](JsonObject &root, int originId) { servoEvent(root, originId); });
-        _socket->onEvent(EVENT_STATE, [&](JsonObject &root, int originId) { stateEvent(root, originId); });
     }
 
     void servoEvent(JsonObject &root, int originId) {
@@ -47,8 +30,6 @@ class ServoController {
         syncAngles(String(originId));
     }
 
-    void stateEvent(JsonObject &root, int originId) { (root["active"] | false) ? activate() : deactivate(); }
-
     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],
@@ -105,3 +86,5 @@ class ServoController {
     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
diff --git a/esp32/platformio.ini b/esp32/platformio.ini
index ee06c8a1..276a4ba2 100644
--- a/esp32/platformio.ini
+++ b/esp32/platformio.ini
@@ -44,6 +44,8 @@ build_flags =
 	-D FT_CAMERA=1
 	-D CAMERA_MODEL_ESP32S3_EYE=1
 	-D WS2812_PIN=48
+	-D USS_LEFT_PIN=1
+	-D USS_RIGHT_PIN=14
 	-D SDA_PIN=47
 	-D SCL_PIN=21