can now recieve visemes through websocket

PROTOCOL_MIGRATION.md

@@ -112,6 +112,9 @@ ushort Crc16Ccitt(byte[] data)
 | `VSET` | Set motor positions for a viseme |
 | `VSME` | Trigger viseme (fire-and-forget) |
 | `STAT` | System state/heartbeat |
+| `FACE` | Face detection data (Radxa → host/robot, via WiFi) |
+| `ALIV` | Remote component alive/heartbeat (component ID + status) |
+| `SSET` | Settings set/dump (read/write individual settings by ID) |
 | `ACK!` | Acknowledge (success) |
 | `NACK` | Negative acknowledge (failure) |
 | `BOOT` | Enter bootloader |
@@ -277,6 +280,39 @@ For each of 3 targets:
 ```
 *Sent automatically when radar streaming is enabled*
 
+#### `FACE` - Face Detection Data (Radxa → host/robot via WiFi)
+**Payload:**
+```
+[face_count: 1 byte]   // 0–N faces
+For each face:
+  [x: 2 bytes LE, signed]   // center-relative pixels (0,0 = image center)
+  [y: 2 bytes LE, signed]
+  [w: 2 bytes LE]           // bounding box width
+  [h: 2 bytes LE]           // bounding box height
+  [conf: 1 byte]            // confidence × 255 (0–255)
+```
+
+**Notes:**
+- Sent from Radxa over WebSocket when faces are detected
+- Radxa runs `face_server.py` (WebSocket server); robot/host connects to receive packets
+- x, y are offset from image center: positive = right/down, negative = left/up
+- Same packet format as serial (0xA5 0x5A + TAG + LENGTH + SEQ + PAYLOAD + CRC16)
+
+#### `ALIV` - Remote Component Alive (Radxa/other → host/robot via WiFi)
+**Payload:**
+```
+[component_id: 1 byte]   // Assigned ID for each remote component
+[alive: 1 byte]         // 0 = not alive / stream disconnected, 1 = alive
+// Future: extended payload for other devices
+```
+
+**Component IDs:**
+- `3` = Face detection (Radxa `face_server.py` – alive = MJPEG stream connected)
+
+**Notes:**
+- Sent every 2 seconds by each remote component
+- Use `alive` to detect when a component has lost its connection and is retrying
+
 ---
 
 ### Behaviors
@@ -290,7 +326,7 @@ For each of 3 targets:
 **Response:** `ACK!` on success, `NACK` on failure
 
 **Behavior IDs:**
-- `1` = Focus (radar tracking with eye motors 14 & 15)
+- `1` = Focus (face tracking with eye motors 14 & 15)
 - `2` = Idle (perlin noise motion for all motors, ±500 range from center)
 - `3` = Viseme (mouth motor control for speech)
 
@@ -386,6 +422,78 @@ For each motor:
 
 ---
 
+### Settings
+
+#### `SSET` - Settings Set/Dump (host ↔ device)
+
+**Write a single setting:**
+**Request:**
+```
+[setting_id: 2 bytes LE]
+[data: N bytes]          // 2 bytes for numeric, variable for strings
+```
+**Response:** `ACK!` on success, `NACK` if unknown setting ID
+
+**Notes:** Writing a WiFi/WebSocket setting triggers an automatic reconnect.
+
+**Dump all settings:**
+**Request:** Empty payload (0 bytes)
+**Response:** `SSET` packet:
+```
+[count: 2 bytes LE]
+For each setting:
+  [setting_id: 2 bytes LE]
+  [data_len: 2 bytes LE]
+  [data: data_len bytes]
+```
+
+**Value encoding:**
+- `uint8`/`uint16`/`bool`: `data_len=2`, stored as uint16 LE
+- `float` (0.0–65.535): `data_len=2`, stored as `value × 1000` (e.g., 0.15 → 150)
+- `int16` (signed): `data_len=2`, stored as uint16 reinterpret (e.g., -140 → 0xFF74)
+- `string`: `data_len=N`, raw UTF-8 bytes (no null terminator)
+
+**Focus Behavior Setting IDs (0x0500–0x0512):**
+
+| ID | Name | Type | Default | Description |
+|----|------|------|---------|-------------|
+| `0x0500` | FOCUS_EYE_MOTOR_1 | uint8 | 14 | Eye motor 1 ID |
+| `0x0501` | FOCUS_EYE_MOTOR_2 | uint8 | 15 | Eye motor 2 ID |
+| `0x0502` | FOCUS_NECK_MOTOR | uint8 | 27 | Neck yaw motor ID |
+| `0x0503` | FOCUS_EYE_CENTER | uint16 | 2200 | Eye servo center position |
+| `0x0504` | FOCUS_EYE_MIN | uint16 | 1700 | Eye servo min position |
+| `0x0505` | FOCUS_EYE_MAX | uint16 | 2500 | Eye servo max position |
+| `0x0506` | FOCUS_NECK_CENTER | uint16 | 2000 | Neck servo center position |
+| `0x0507` | FOCUS_NECK_MIN | uint16 | 1000 | Neck servo min position |
+| `0x0508` | FOCUS_NECK_MAX | uint16 | 3000 | Neck servo max position |
+| `0x0509` | FOCUS_FACE_X_MIN | int16 | -140 | Face detection X min (pixels) |
+| `0x050A` | FOCUS_FACE_X_MAX | int16 | 140 | Face detection X max (pixels) |
+| `0x050B` | FOCUS_EYE_SPEED | float×1000 | 150 | Eye dart speed (0–1) |
+| `0x050C` | FOCUS_NECK_SPEED | float×1000 | 20 | Neck follow speed (0–1) |
+| `0x050D` | FOCUS_EYE_RETURN_SPEED | float×1000 | 50 | Eye return-to-center speed |
+| `0x050E` | FOCUS_NECK_DELAY_MS | uint16 | 500 | Neck start delay (ms) |
+| `0x050F` | FOCUS_NECK_CONTRIBUTION | float×1000 | 700 | Neck offset coverage (0–1) |
+| `0x0510` | FOCUS_NECK_INVERT | bool | 1 | Invert neck motor direction |
+| `0x0511` | FOCUS_EYE_CENTERING | float×1000 | 30 | Eye centering speed (no face) |
+| `0x0512` | FOCUS_NECK_CENTERING | float×1000 | 20 | Neck centering speed (no face) |
+
+**WiFi / WebSocket Setting IDs (0x0600–0x0604):**
+
+| ID | Name | Type | Default | Description |
+|----|------|------|---------|-------------|
+| `0x0600` | WIFI_SSID | string | "Police Surveillance Van" | WiFi network name (max 32 chars) |
+| `0x0601` | WIFI_PASSWORD | string | "ourpassword" | WiFi password (max 64 chars) |
+| `0x0602` | WIFI_HOST | string | "192.168.1.206" | WebSocket server IP/hostname (max 63 chars) |
+| `0x0603` | WIFI_PORT | uint16 | 5001 | WebSocket server port |
+| `0x0604` | WIFI_PATH | string | "/" | WebSocket URL path (max 31 chars) |
+
+**Notes:**
+- All settings are persisted to flash on write
+- The dump includes all registered settings with a length prefix per entry
+- Setting ranges are extensible: `0x0500` = Focus, `0x0600` = WiFi, future = `0x0700+`
+
+---
+
 ### System
 
 #### `STAT` - System State/Heartbeat (device → host)
@@ -400,6 +508,8 @@ For each motor:
 - Bit 2: Motor streaming active
 - Bit 3: IMU streaming active
 - Bit 4: Radar streaming active
+- Bit 5: Face streaming active
+- Bit 6: Face detection remote alive
 
 *Sent automatically every 1 second*
 
@@ -542,4 +652,22 @@ Key files:
 - `protocol.h/cpp` - Packet format and CRC implementation
 - `commands.h/cpp` - Command handlers
 - `sensors.h/cpp` - IMU and radar drivers
+- `websocket_client.h/cpp` - WiFi, WebSocket client + server
+
+---
+
+## WebSocket Server
+
+The firmware runs a WebSocket server on port **81** alongside the outbound client.
+Any device on the same WiFi network can connect and send protocol-framed packets.
+
+**URL:** `ws://<robot-ip>:81/`
+
+All commands from the serial protocol are supported over WebSocket:
+`VSME`, `MSET`, `BHVR`, `SSET`, `FPLY`, `FSTP`, etc.
+
+Packets must use the same binary framing as serial:
+`0xA5 0x5A + TAG(4) + LEN(2) + SEQ(2) + PAYLOAD(N) + CRC16(2)`
+
+Up to 2 simultaneous WebSocket client connections are supported.
 

behaviors.cpp

@@ -36,176 +36,124 @@ void Behavior::clearMotors() {
 
 FocusBehavior::FocusBehavior() {
   isActive = false;
-  eyePosition = EYE_POSITION_CENTER;
-  neckPosition = NECK_POSITION_CENTER;
-  targetEyePosition = EYE_POSITION_CENTER;
-  targetNeckPosition = NECK_POSITION_CENTER;
-  targetDetectedTime = 0;
-  neckStartTime = 0;
-  neckRotating = false;
+  eyePosition = settings.eyeCenter;
+  neckPosition = settings.neckCenter;
+  neckNormalized = 0.0f;
+  faceDetectedTime = 0;
+  faceWasPresent = false;
   
-  // Add motors 14, 15, and 27 to controlled list
-  addMotor(FOCUS_MOTOR_1);
-  addMotor(FOCUS_MOTOR_2);
-  addMotor(NECK_MOTOR);
+  // Add motors to controlled list
+  addMotor(settings.eyeMotor1);
+  addMotor(settings.eyeMotor2);
+  addMotor(settings.neckMotor);
 }
 
 bool FocusBehavior::update() {
-  // Check radar for valid targets
-  uint8_t targetCount = radar.getTargetCount();
+  uint8_t faceCount = faceDetect.getFaceCount();
   unsigned long now = millis();
   
-  if (targetCount == 0 || !radar.getTarget(0).valid) {
-    // No target - return to center
+  // ---- No face detected ----
+  if (faceCount == 0 || !faceDetect.getFace(0).valid) {
     isActive = false;
-    targetEyePosition = EYE_POSITION_CENTER;
-    targetNeckPosition = NECK_POSITION_CENTER;
-    targetDetectedTime = 0;
-    neckRotating = false;
+    faceWasPresent = false;
     
-    // Smoothly interpolate eyes to center
-    eyePosition = lerp(eyePosition, EYE_POSITION_CENTER, INTERPOLATION_SPEED);
-    
-    // Keep neck at center (no movement)
-    neckPosition = NECK_POSITION_CENTER;
+    // Smoothly return eyes and neck to center
+    eyePosition = lerp(eyePosition, settings.eyeCenter, settings.eyeCenteringSpeed);
+    neckNormalized = lerpf(neckNormalized, 0.0f, settings.neckCenteringSpeed);
+    neckPosition = normalizedToServo(
+      settings.neckInvert ? -neckNormalized : neckNormalized,
+      settings.neckCenter, settings.neckMin, settings.neckMax
+    );
     
     return false;
   }
 
-  // Get first valid target
-  const RadarTarget& target = radar.getTarget(0);
-  
-  if (!target.valid) {
-    isActive = false;
-    targetEyePosition = EYE_POSITION_CENTER;
-    targetNeckPosition = NECK_POSITION_CENTER;
-    targetDetectedTime = 0;
-    neckRotating = false;
-    return false;
-  }
-
-  // Active tracking - calculate target positions from radar angle
+  // ---- Face detected ----
   isActive = true;
-  uint16_t targetEyePos = calculateEyePositionFromRadarAngle(target.angle);
+  const DetectedFace& face = faceDetect.getFace(0);
+
+  // Track when we first saw a face (for neck delay)
+  if (!faceWasPresent) {
+    faceDetectedTime = now;
+    faceWasPresent = true;
+  }
+
+  // Normalize face x to -1..+1
+  float faceNorm = (float)face.x / settings.faceXMax;
+  if (faceNorm < -1.0f) faceNorm = -1.0f;
+  if (faceNorm >  1.0f) faceNorm =  1.0f;
+
+  // ---- Neck: smoothly follow the target after a delay ----
+  float neckTarget = faceNorm * settings.neckContribution;
   
-  // Eyes track immediately
-  targetEyePosition = targetEyePos;
-  
-  // Neck disabled for now - keep it centered
-  targetNeckPosition = NECK_POSITION_CENTER;
-  neckRotating = false;
-  
-  // Smoothly interpolate eye position toward target
-  eyePosition = lerp(eyePosition, targetEyePosition, INTERPOLATION_SPEED);
-  
-  // Keep neck at center (no movement)
-  neckPosition = NECK_POSITION_CENTER;
+  if (now - faceDetectedTime >= settings.neckDelayMs) {
+    // Neck is allowed to move - smoothly interpolate toward target
+    neckNormalized = lerpf(neckNormalized, neckTarget, settings.neckSpeed);
+  }
+  // else: neck stays where it is during the delay period
+
+  // Convert neck normalized position to servo units
+  neckPosition = normalizedToServo(
+    settings.neckInvert ? -neckNormalized : neckNormalized,
+    settings.neckCenter, settings.neckMin, settings.neckMax
+  );
+
+  // ---- Eyes: dart to the remainder that the neck hasn't covered ----
+  // The eyes compensate for whatever offset the neck hasn't reached yet
+  // As the neck catches up, this remainder shrinks toward 0 (eyes center)
+  float eyeNorm = faceNorm - neckNormalized;
   
+  // Clamp eye normalized to -1..+1
+  if (eyeNorm < -1.0f) eyeNorm = -1.0f;
+  if (eyeNorm >  1.0f) eyeNorm =  1.0f;
+
+  // Convert to servo position and interpolate quickly
+  uint16_t eyeTarget = normalizedToServo(eyeNorm, settings.eyeCenter, settings.eyeMin, settings.eyeMax);
+  eyePosition = lerp(eyePosition, eyeTarget, settings.eyeSpeed);
+
   return true;
 }
 
 bool FocusBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
-  // Provide position for eyes (motors 14 and 15)
-  if (motorID == FOCUS_MOTOR_1 || motorID == FOCUS_MOTOR_2) {
+  if (motorID == settings.eyeMotor1 || motorID == settings.eyeMotor2) {
     position = eyePosition;
     return true;
   }
-  
-  // Provide position for neck (motor 27)
-  if (motorID == NECK_MOTOR) {
+  if (motorID == settings.neckMotor) {
     position = neckPosition;
     return true;
   }
-  
   return false;
 }
 
-uint16_t FocusBehavior::calculateEyePositionFromRadarAngle(float radarAngle) {
-  // Calculate eye motor position from radar angle (in degrees)
-  // Angle range: -50 to +50 degrees, mapped to full eye range (1700-2500, center 2200)
-  
-  constexpr float ANGLE_MIN = -50.0f;
-  constexpr float ANGLE_MAX = 50.0f;
-  
-  // Clamp angle to -50 to +50 range
-  if (radarAngle < ANGLE_MIN) radarAngle = ANGLE_MIN;
-  if (radarAngle > ANGLE_MAX) radarAngle = ANGLE_MAX;
-  
-  // Normalize angle to -1.0 to 1.0 range
-  float normalized = radarAngle / 50.0f;
-  
-  // Calculate range from center in each direction
-  // Left range: 2200 - 1700 = 500, Right range: 2500 - 2200 = 300
-  float rangeLeft = (float)(EYE_POSITION_CENTER - EYE_POSITION_MIN);   // 500
-  float rangeRight = (float)(EYE_POSITION_MAX - EYE_POSITION_CENTER);  // 300
-  
-  // Use different ranges for left (negative) and right (positive) to use full range
-  float positionFloat;
-  if (normalized < 0.0f) {
-    // Negative angle: use left range (500 units)
-    positionFloat = (float)EYE_POSITION_CENTER + (normalized * rangeLeft);
+uint16_t FocusBehavior::normalizedToServo(float n, uint16_t center, uint16_t min, uint16_t max) const {
+  // Map a normalized value (-1..+1) to servo range, handling asymmetric ranges
+  float rangeNeg = (float)(center - min);
+  float rangePos = (float)(max - center);
+
+  float posFloat;
+  if (n < 0.0f) {
+    posFloat = (float)center + (n * rangeNeg);
   } else {
-    // Positive angle: use right range (300 units)
-    positionFloat = (float)EYE_POSITION_CENTER + (normalized * rangeRight);
+    posFloat = (float)center + (n * rangePos);
   }
-  
-  // Convert to int16_t first to handle negative values, then clamp
-  int16_t position = (int16_t)positionFloat;
 
-  // Clamp to valid range (1700 to 2500)
-  if (position < (int16_t)EYE_POSITION_MIN) position = (int16_t)EYE_POSITION_MIN;
-  if (position > (int16_t)EYE_POSITION_MAX) position = (int16_t)EYE_POSITION_MAX;
-
-  return (uint16_t)position;
+  int16_t pos = (int16_t)posFloat;
+  if (pos < (int16_t)min) pos = (int16_t)min;
+  if (pos > (int16_t)max) pos = (int16_t)max;
+  return (uint16_t)pos;
 }
 
-uint16_t FocusBehavior::calculateNeckPositionFromRadarAngle(float radarAngle) {
-  // Calculate neck motor position from radar angle (in degrees)
-  // Angle range: approximately -45 to +45 degrees (typical radar FOV)
-  // Map to neck motor position range: 1000 to 3000 (center 2000)
-  // NOTE: Rotation is inverted for neck motor
-  
-  // Clamp angle to reasonable range (can extend later if needed)
-  constexpr float ANGLE_MIN = -45.0f;
-  constexpr float ANGLE_MAX = 45.0f;
-  
-  if (radarAngle < ANGLE_MIN) radarAngle = ANGLE_MIN;
-  if (radarAngle > ANGLE_MAX) radarAngle = ANGLE_MAX;
-  
-  // Normalize angle to -1.0 to 1.0 range, then invert for neck motor
-  float normalizedAngle = -(radarAngle / ANGLE_MAX);
-  
-  // Calculate range from center
-  float rangeLeft = NECK_POSITION_CENTER - NECK_POSITION_MIN;   // 1000
-  float rangeRight = NECK_POSITION_MAX - NECK_POSITION_CENTER;  // 1000
-  
-  uint16_t position;
-  if (normalizedAngle < 0.0f) {
-    // Left side: use left range
-    position = NECK_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeLeft);
-  } else {
-    // Right side: use right range
-    position = NECK_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeRight);
-  }
-
-  // Clamp to valid range
-  if (position < NECK_POSITION_MIN) position = NECK_POSITION_MIN;
-  if (position > NECK_POSITION_MAX) position = NECK_POSITION_MAX;
-
-  return position;
+float FocusBehavior::lerpf(float current, float target, float t) {
+  float diff = target - current;
+  if (fabs(diff) < 0.001f) return target;
+  return current + diff * t;
 }
 
 uint16_t FocusBehavior::lerp(uint16_t current, uint16_t target, float t) {
-  // Linear interpolation with clamping to prevent overshoot
   int16_t diff = (int16_t)target - (int16_t)current;
-  int16_t delta = (int16_t)(diff * t);
-  
-  // If difference is very small, snap to target
-  if (abs(diff) < 2) {
-    return target;
-  }
-  
-  return (uint16_t)(current + delta);
+  if (abs(diff) < 2) return target;
+  return (uint16_t)((int16_t)current + (int16_t)(diff * t));
 }
 
 // ============================================================================
@@ -624,6 +572,7 @@ bool VisemeBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
 // ============================================================================
 
 BehaviorManager behaviorManager;
+FocusBehavior focusBehavior;
 VisemeBehavior visemeBehavior;
 
 BehaviorManager::BehaviorManager() {

behaviors.h

@@ -10,7 +10,7 @@
 // ============================================================================
 
 enum BehaviorID : uint8_t {
-  BEHAVIOR_FOCUS = 1,  // Focus behavior (radar tracking)
+  BEHAVIOR_FOCUS = 1,  // Focus behavior (face tracking)
   BEHAVIOR_IDLE = 2,   // Idle behavior (perlin noise for all motors)
   BEHAVIOR_VISEME = 3, // Viseme behavior (mouth motor positions)
 };
@@ -49,59 +49,136 @@ protected:
 };
 
 // ============================================================================
-// Focus Behavior - Tracks radar targets with eyes/neck
+// Focus Behavior - Tracks faces with eyes/neck via FaceDetect sensor
 // ============================================================================
 
+// Setting IDs for SSET protocol command
+// Float values are transmitted as fixed-point × 1000 (e.g., 0.15 → 150)
+// Signed values are transmitted as int16 reinterpreted as uint16
+namespace SettingID {
+  // Focus: Motor IDs
+  constexpr uint16_t FOCUS_EYE_MOTOR_1       = 0x0500;
+  constexpr uint16_t FOCUS_EYE_MOTOR_2       = 0x0501;
+  constexpr uint16_t FOCUS_NECK_MOTOR        = 0x0502;
+  // Focus: Eye servo range
+  constexpr uint16_t FOCUS_EYE_CENTER        = 0x0503;
+  constexpr uint16_t FOCUS_EYE_MIN           = 0x0504;
+  constexpr uint16_t FOCUS_EYE_MAX           = 0x0505;
+  // Focus: Neck servo range
+  constexpr uint16_t FOCUS_NECK_CENTER       = 0x0506;
+  constexpr uint16_t FOCUS_NECK_MIN          = 0x0507;
+  constexpr uint16_t FOCUS_NECK_MAX          = 0x0508;
+  // Focus: Face x range (int16, signed)
+  constexpr uint16_t FOCUS_FACE_X_MIN        = 0x0509;
+  constexpr uint16_t FOCUS_FACE_X_MAX        = 0x050A;
+  // Focus: Interpolation speeds (float × 1000)
+  constexpr uint16_t FOCUS_EYE_SPEED         = 0x050B;
+  constexpr uint16_t FOCUS_NECK_SPEED        = 0x050C;
+  constexpr uint16_t FOCUS_EYE_RETURN_SPEED  = 0x050D;
+  // Focus: Neck delay (ms, uint16)
+  constexpr uint16_t FOCUS_NECK_DELAY_MS     = 0x050E;
+  // Focus: Neck contribution (float × 1000)
+  constexpr uint16_t FOCUS_NECK_CONTRIBUTION = 0x050F;
+  // Focus: Neck invert (0 or 1)
+  constexpr uint16_t FOCUS_NECK_INVERT       = 0x0510;
+  // Focus: Centering speeds (float × 1000)
+  constexpr uint16_t FOCUS_EYE_CENTERING     = 0x0511;
+  constexpr uint16_t FOCUS_NECK_CENTERING    = 0x0512;
+  
+  constexpr uint16_t FOCUS_FIRST = 0x0500;
+  constexpr uint16_t FOCUS_LAST  = 0x0512;
+  constexpr uint16_t FOCUS_COUNT = FOCUS_LAST - FOCUS_FIRST + 1;
+  
+  // WiFi / WebSocket settings (0x0600 range)
+  constexpr uint16_t WIFI_SSID             = 0x0600;  // string (max 32 chars)
+  constexpr uint16_t WIFI_PASSWORD         = 0x0601;  // string (max 64 chars)
+  constexpr uint16_t WIFI_HOST             = 0x0602;  // string (max 63 chars)
+  constexpr uint16_t WIFI_PORT             = 0x0603;  // uint16
+  constexpr uint16_t WIFI_PATH             = 0x0604;  // string (max 31 chars)
+  
+  constexpr uint16_t WIFI_FIRST = 0x0600;
+  constexpr uint16_t WIFI_LAST  = 0x0604;
+  constexpr uint16_t WIFI_COUNT = WIFI_LAST - WIFI_FIRST + 1;
+}
+
+// Tuneable settings - all values exposed for external adjustment
+struct FocusSettings {
+  // Motor IDs
+  uint8_t eyeMotor1 = 14;
+  uint8_t eyeMotor2 = 15;
+  uint8_t neckMotor = 27;
+
+  // Eye motor position range
+  uint16_t eyeCenter = 2200;
+  uint16_t eyeMin    = 1700;
+  uint16_t eyeMax    = 2500;
+
+  // Neck motor position range
+  uint16_t neckCenter = 2000;
+  uint16_t neckMin    = 1000;
+  uint16_t neckMax    = 3000;
+
+  // Face detection x range (pixels, center-relative)
+  float faceXMin = -140.0f;
+  float faceXMax =  140.0f;
+
+  // Interpolation speeds (0-1 per update, higher = faster)
+  float eyeSpeed      = 0.15f;   // Eyes dart quickly to target
+  float neckSpeed     = 0.02f;   // Neck follows smoothly / slowly
+  float eyeReturnSpeed = 0.05f;  // Speed eyes center as neck catches up
+
+  // Neck starts moving after this delay (ms) from first detecting a target
+  unsigned long neckDelayMs = 500;
+
+  // How much of the face offset the neck should try to cover (0-1)
+  // 1.0 = neck tries to fully face the target, 0.5 = neck covers half
+  float neckContribution = 0.7f;
+
+  // Invert neck direction (set true if neck motor is wired backwards)
+  bool neckInvert = true;
+
+  // Return-to-center speeds when no face detected
+  float eyeCenteringSpeed = 0.03f;
+  float neckCenteringSpeed = 0.02f;
+};
+
 class FocusBehavior : public Behavior {
 public:
   FocusBehavior();
 
-  // Update behavior - check radar for targets
+  // Update behavior - check face detection for targets
   bool update() override;
 
   // Get motor position for a controlled motor
   bool getMotorPosition(uint8_t motorID, uint16_t& position) override;
 
-private:
-  bool isActive;
-  uint16_t eyePosition;      // Current interpolated position for motors 14 and 15
-  uint16_t neckPosition;     // Current interpolated position for motor 27
-  
-  // Target positions
-  uint16_t targetEyePosition;
-  uint16_t targetNeckPosition;
-  
-  // Timing
-  unsigned long targetDetectedTime;  // When target was first detected
-  unsigned long neckStartTime;       // When neck rotation should start
-  bool neckRotating;                 // Whether neck is actively rotating
-  
-  // Configuration
-  static constexpr uint8_t FOCUS_MOTOR_1 = 14;  // Eye motor 1
-  static constexpr uint8_t FOCUS_MOTOR_2 = 15;  // Eye motor 2
-  static constexpr uint8_t NECK_MOTOR = 27;     // Neck motor
-  
-  // Eye motor position range (motors 14 and 15)
-  static constexpr uint16_t EYE_POSITION_CENTER = 2200;
-  static constexpr uint16_t EYE_POSITION_MIN = 1700;
-  static constexpr uint16_t EYE_POSITION_MAX = 2500;
-  
-  // Neck motor position range (motor 27)
-  static constexpr uint16_t NECK_POSITION_CENTER = 2000;
-  static constexpr uint16_t NECK_POSITION_MIN = 1000;
-  static constexpr uint16_t NECK_POSITION_MAX = 3000;
-  
-  static constexpr unsigned long NECK_DELAY_MS = 1000;  // 1 second delay before neck moves
-  static constexpr float INTERPOLATION_SPEED = 0.05f;   // Smooth interpolation factor for eyes (0-1, higher = faster)
-  static constexpr float NECK_INTERPOLATION_SPEED = 0.03f;  // Slower interpolation for neck (smoother motion)
-  static constexpr float EYE_CENTERING_SPEED = 0.03f;   // Speed at which eyes center when neck rotates
+  // Access settings for external tuning
+  FocusSettings& getSettings() { return settings; }
+  const FocusSettings& getSettings() const { return settings; }
 
-  // Calculate motor position from radar angle (in degrees)
-  uint16_t calculateEyePositionFromRadarAngle(float radarAngle);
-  uint16_t calculateNeckPositionFromRadarAngle(float radarAngle);
+private:
+  FocusSettings settings;
+
+  bool isActive;
   
-  // Smooth interpolation helper (linear interpolation)
-  uint16_t lerp(uint16_t current, uint16_t target, float t);
+  // Current smoothed positions (servo units)
+  uint16_t eyePosition;
+  uint16_t neckPosition;
+  
+  // Current normalized offset the neck has reached (-1 to +1)
+  // This tracks how far the neck has rotated toward the target
+  float neckNormalized;
+
+  // Timing
+  unsigned long faceDetectedTime;  // When face was first seen (for neck delay)
+  bool faceWasPresent;             // Was a face present last frame?
+
+  // Map a normalized value (-1..+1) to an asymmetric servo range
+  uint16_t normalizedToServo(float n, uint16_t center, uint16_t min, uint16_t max) const;
+
+  // Smooth interpolation helpers
+  static float lerpf(float current, float target, float t);
+  static uint16_t lerp(uint16_t current, uint16_t target, float t);
 };
 
 // ============================================================================
@@ -207,7 +284,7 @@ private:
   std::vector<Viseme> visemes;
   
   // Configuration
-  static constexpr unsigned long TIMEOUT_MS = 3000;  // 3 second timeout
+  static constexpr unsigned long TIMEOUT_MS = 200;  // 3 second timeout
   static constexpr uint16_t DEFAULT_POSITION = 2047; // Center/rest position
   
   // Helper to find viseme by ID
@@ -261,5 +338,6 @@ private:
 // Global behavior manager instance
 extern BehaviorManager behaviorManager;
 
-// Global viseme behavior instance (for command access)
+// Global behavior instances (for command/config access)
+extern FocusBehavior focusBehavior;
 extern VisemeBehavior visemeBehavior;

commands.cpp

@@ -2,6 +2,7 @@
 #include "nodegraph.h"
 #include "sensors.h"
 #include "behaviors.h"
+#include "websocket_client.h"
 #include "esp_system.h"
 #include "soc/rtc_cntl_reg.h"
 #include <vector>
@@ -86,10 +87,10 @@ bool MotorStreamState::shouldStream() {
 // ============================================================================
 
 void dispatchCommand() {
-  const char* tag = getReceivedTag();
-  const uint8_t* payload = getReceivedPayload();
-  uint16_t len = getReceivedPayloadLen();
-  
+  dispatchCommand(getReceivedTag(), getReceivedPayload(), getReceivedPayloadLen());
+}
+
+void dispatchCommand(const char tag[4], const uint8_t* payload, uint16_t len) {
   // Identity & Config
   if (tagMatches(tag, Tag::IDENT)) {
     handleIdent(payload, len);
@@ -152,6 +153,10 @@ void dispatchCommand() {
   else if (tagMatches(tag, Tag::VSME)) {
     handleVisemeTrigger(payload, len);
   }
+  // Settings
+  else if (tagMatches(tag, Tag::SSET)) {
+    handleSettingsSet(payload, len);
+  }
   // System
   else if (tagMatches(tag, Tag::BOOT)) {
     handleBootloader(payload, len);
@@ -467,7 +472,7 @@ void handleBehavior(const uint8_t* payload, uint16_t len) {
   behaviorManager.setBehaviorEnabled(static_cast<BehaviorID>(behaviorID), enabled);
   
   // Save config to persist the behavior state change
-  config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
+  config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
   
   // Send acknowledgment
   sendAck(Tag::BHVR);
@@ -578,7 +583,7 @@ void handleVisemeAdd(const uint8_t* payload, uint16_t len) {
   uint8_t newID = visemeBehavior.addViseme(label);
   
   // Save config to persist the new viseme
-  config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
+  config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
   
   // Send ACK with the new ID
   uint8_t ackPayload[1] = { newID };
@@ -596,7 +601,7 @@ void handleVisemeDelete(const uint8_t* payload, uint16_t len) {
   
   if (visemeBehavior.deleteViseme(visemeID)) {
     // Save config to persist the deletion
-    config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
+    config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
     sendAck(Tag::VDEL);
   } else {
     sendNack(Tag::VDEL, "Viseme not found");
@@ -640,7 +645,7 @@ void handleVisemeSet(const uint8_t* payload, uint16_t len) {
   // Use createOrUpdateViseme so VSET can create new visemes or update existing ones
   if (visemeBehavior.createOrUpdateViseme(visemeID, label, positions)) {
     // Save config to persist the changes
-    config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
+    config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
     sendAck(Tag::VSET);
   } else {
     sendNack(Tag::VSET, "Failed to update viseme");
@@ -827,6 +832,211 @@ void handleMotorStream(const uint8_t* payload, uint16_t len) {
 // ============================================================================
 // System Handlers
 // ============================================================================
+// Settings (SSET) - Read/write individual settings by ID
+// Supports numeric (uint16) and string values.
+// Dump format: [count:2][id:2][len:2][data:N]...
+// Write format: [id:2][data:N] (len=2 for uint16, len>2 for strings)
+// ============================================================================
+
+// Returns true if this setting ID is a string type
+static bool isStringSetting(uint16_t id) {
+  return id == SettingID::WIFI_SSID || id == SettingID::WIFI_PASSWORD ||
+         id == SettingID::WIFI_HOST || id == SettingID::WIFI_PATH;
+}
+
+// Get a numeric setting value by ID. Returns true if valid.
+static bool getSettingValue(uint16_t id, uint16_t& value) {
+  FocusSettings& fs = focusBehavior.getSettings();
+  
+  switch (id) {
+    case SettingID::FOCUS_EYE_MOTOR_1:       value = fs.eyeMotor1; break;
+    case SettingID::FOCUS_EYE_MOTOR_2:       value = fs.eyeMotor2; break;
+    case SettingID::FOCUS_NECK_MOTOR:        value = fs.neckMotor; break;
+    case SettingID::FOCUS_EYE_CENTER:        value = fs.eyeCenter; break;
+    case SettingID::FOCUS_EYE_MIN:           value = fs.eyeMin; break;
+    case SettingID::FOCUS_EYE_MAX:           value = fs.eyeMax; break;
+    case SettingID::FOCUS_NECK_CENTER:       value = fs.neckCenter; break;
+    case SettingID::FOCUS_NECK_MIN:          value = fs.neckMin; break;
+    case SettingID::FOCUS_NECK_MAX:          value = fs.neckMax; break;
+    case SettingID::FOCUS_FACE_X_MIN:        value = (uint16_t)(int16_t)fs.faceXMin; break;
+    case SettingID::FOCUS_FACE_X_MAX:        value = (uint16_t)(int16_t)fs.faceXMax; break;
+    case SettingID::FOCUS_EYE_SPEED:         value = (uint16_t)(fs.eyeSpeed * 1000.0f); break;
+    case SettingID::FOCUS_NECK_SPEED:        value = (uint16_t)(fs.neckSpeed * 1000.0f); break;
+    case SettingID::FOCUS_EYE_RETURN_SPEED:  value = (uint16_t)(fs.eyeReturnSpeed * 1000.0f); break;
+    case SettingID::FOCUS_NECK_DELAY_MS:     value = (uint16_t)fs.neckDelayMs; break;
+    case SettingID::FOCUS_NECK_CONTRIBUTION: value = (uint16_t)(fs.neckContribution * 1000.0f); break;
+    case SettingID::FOCUS_NECK_INVERT:       value = fs.neckInvert ? 1 : 0; break;
+    case SettingID::FOCUS_EYE_CENTERING:     value = (uint16_t)(fs.eyeCenteringSpeed * 1000.0f); break;
+    case SettingID::FOCUS_NECK_CENTERING:    value = (uint16_t)(fs.neckCenteringSpeed * 1000.0f); break;
+    // WiFi numeric
+    case SettingID::WIFI_PORT:               value = wifiSettings.port; break;
+    default: return false;
+  }
+  return true;
+}
+
+// Get a string setting by ID. Returns pointer to string, or nullptr.
+static const char* getSettingString(uint16_t id) {
+  switch (id) {
+    case SettingID::WIFI_SSID:     return wifiSettings.ssid;
+    case SettingID::WIFI_PASSWORD: return wifiSettings.password;
+    case SettingID::WIFI_HOST:     return wifiSettings.host;
+    case SettingID::WIFI_PATH:     return wifiSettings.path;
+    default: return nullptr;
+  }
+}
+
+// Set a numeric setting value by ID. Returns true if valid.
+static bool setSettingValue(uint16_t id, uint16_t value) {
+  FocusSettings& fs = focusBehavior.getSettings();
+  
+  switch (id) {
+    case SettingID::FOCUS_EYE_MOTOR_1:       fs.eyeMotor1 = (uint8_t)value; break;
+    case SettingID::FOCUS_EYE_MOTOR_2:       fs.eyeMotor2 = (uint8_t)value; break;
+    case SettingID::FOCUS_NECK_MOTOR:        fs.neckMotor = (uint8_t)value; break;
+    case SettingID::FOCUS_EYE_CENTER:        fs.eyeCenter = value; break;
+    case SettingID::FOCUS_EYE_MIN:           fs.eyeMin = value; break;
+    case SettingID::FOCUS_EYE_MAX:           fs.eyeMax = value; break;
+    case SettingID::FOCUS_NECK_CENTER:       fs.neckCenter = value; break;
+    case SettingID::FOCUS_NECK_MIN:          fs.neckMin = value; break;
+    case SettingID::FOCUS_NECK_MAX:          fs.neckMax = value; break;
+    case SettingID::FOCUS_FACE_X_MIN:        fs.faceXMin = (float)(int16_t)value; break;
+    case SettingID::FOCUS_FACE_X_MAX:        fs.faceXMax = (float)(int16_t)value; break;
+    case SettingID::FOCUS_EYE_SPEED:         fs.eyeSpeed = (float)value / 1000.0f; break;
+    case SettingID::FOCUS_NECK_SPEED:        fs.neckSpeed = (float)value / 1000.0f; break;
+    case SettingID::FOCUS_EYE_RETURN_SPEED:  fs.eyeReturnSpeed = (float)value / 1000.0f; break;
+    case SettingID::FOCUS_NECK_DELAY_MS:     fs.neckDelayMs = (unsigned long)value; break;
+    case SettingID::FOCUS_NECK_CONTRIBUTION: fs.neckContribution = (float)value / 1000.0f; break;
+    case SettingID::FOCUS_NECK_INVERT:       fs.neckInvert = value != 0; break;
+    case SettingID::FOCUS_EYE_CENTERING:     fs.eyeCenteringSpeed = (float)value / 1000.0f; break;
+    case SettingID::FOCUS_NECK_CENTERING:    fs.neckCenteringSpeed = (float)value / 1000.0f; break;
+    // WiFi numeric
+    case SettingID::WIFI_PORT:               wifiSettings.port = value; break;
+    default: return false;
+  }
+  return true;
+}
+
+// Set a string setting by ID. Returns true if valid.
+static bool setSettingString(uint16_t id, const char* str, uint16_t strLen) {
+  switch (id) {
+    case SettingID::WIFI_SSID:
+      if (strLen >= sizeof(wifiSettings.ssid)) strLen = sizeof(wifiSettings.ssid) - 1;
+      memcpy(wifiSettings.ssid, str, strLen);
+      wifiSettings.ssid[strLen] = '\0';
+      break;
+    case SettingID::WIFI_PASSWORD:
+      if (strLen >= sizeof(wifiSettings.password)) strLen = sizeof(wifiSettings.password) - 1;
+      memcpy(wifiSettings.password, str, strLen);
+      wifiSettings.password[strLen] = '\0';
+      break;
+    case SettingID::WIFI_HOST:
+      if (strLen >= sizeof(wifiSettings.host)) strLen = sizeof(wifiSettings.host) - 1;
+      memcpy(wifiSettings.host, str, strLen);
+      wifiSettings.host[strLen] = '\0';
+      break;
+    case SettingID::WIFI_PATH:
+      if (strLen >= sizeof(wifiSettings.path)) strLen = sizeof(wifiSettings.path) - 1;
+      memcpy(wifiSettings.path, str, strLen);
+      wifiSettings.path[strLen] = '\0';
+      break;
+    default: return false;
+  }
+  return true;
+}
+
+// Helper: write one setting entry into a buffer at offset. Returns new offset.
+// Format: [id:2][len:2][data:N]
+static uint16_t writeDumpEntry(uint8_t* buf, uint16_t offset, uint16_t id, const uint8_t* data, uint16_t dataLen) {
+  buf[offset++] = id & 0xFF;
+  buf[offset++] = (id >> 8) & 0xFF;
+  buf[offset++] = dataLen & 0xFF;
+  buf[offset++] = (dataLen >> 8) & 0xFF;
+  memcpy(&buf[offset], data, dataLen);
+  offset += dataLen;
+  return offset;
+}
+
+void handleSettingsSet(const uint8_t* payload, uint16_t len) {
+  if (len == 0) {
+    // Dump all settings: [count:2][id:2][len:2][data:N]...
+    // Max size: 2 + (FOCUS_COUNT + WIFI_COUNT) * (2+2+64) ≈ 1600 bytes, safe for stack
+    uint8_t buf[1600];
+    uint16_t count = 0;
+    uint16_t offset = 2;  // Skip count, fill later
+    
+    // Dump all numeric focus settings
+    for (uint16_t id = SettingID::FOCUS_FIRST; id <= SettingID::FOCUS_LAST; id++) {
+      uint16_t value;
+      if (getSettingValue(id, value)) {
+        uint8_t vbuf[2] = { (uint8_t)(value & 0xFF), (uint8_t)((value >> 8) & 0xFF) };
+        offset = writeDumpEntry(buf, offset, id, vbuf, 2);
+        count++;
+      }
+    }
+    
+    // Dump WiFi string settings
+    for (uint16_t id = SettingID::WIFI_FIRST; id <= SettingID::WIFI_LAST; id++) {
+      if (isStringSetting(id)) {
+        const char* str = getSettingString(id);
+        if (str) {
+          uint16_t slen = strlen(str);
+          offset = writeDumpEntry(buf, offset, id, (const uint8_t*)str, slen);
+          count++;
+        }
+      } else {
+        // Numeric WiFi setting (port)
+        uint16_t value;
+        if (getSettingValue(id, value)) {
+          uint8_t vbuf[2] = { (uint8_t)(value & 0xFF), (uint8_t)((value >> 8) & 0xFF) };
+          offset = writeDumpEntry(buf, offset, id, vbuf, 2);
+          count++;
+        }
+      }
+    }
+    
+    // Write count at the start
+    buf[0] = count & 0xFF;
+    buf[1] = (count >> 8) & 0xFF;
+    
+    sendPacket(Tag::SSET, buf, offset);
+    return;
+  }
+  
+  if (len >= 4) {
+    // Write a setting: [id:2 LE][data:N]
+    uint16_t id = payload[0] | (payload[1] << 8);
+    uint16_t dataLen = len - 2;
+    const uint8_t* data = &payload[2];
+    
+    bool ok = false;
+    bool needsReconnect = false;
+    
+    if (isStringSetting(id)) {
+      ok = setSettingString(id, (const char*)data, dataLen);
+      needsReconnect = true;
+    } else if (dataLen == 2) {
+      uint16_t value = data[0] | (data[1] << 8);
+      ok = setSettingValue(id, value);
+      if (ok && id == SettingID::WIFI_PORT) needsReconnect = true;
+    }
+    
+    if (ok) {
+      config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
+      sendAck(Tag::SSET);
+      if (needsReconnect) websocketReconnect();
+    } else {
+      sendNack(Tag::SSET, "Unknown setting ID");
+    }
+    return;
+  }
+  
+  sendNack(Tag::SSET, "Invalid payload length");
+}
+
+// ============================================================================
+// System
+// ============================================================================
 
 void handleBootloader(const uint8_t* payload, uint16_t len) {
   sendMessage("Entering bootloader...");

commands.h

@@ -47,6 +47,9 @@ extern MotorStreamState motorStream;
 // Process a received packet - call after receivePacket() returns true
 void dispatchCommand();
 
+// Dispatch a command by tag and payload directly (for WebSocket server, etc.)
+void dispatchCommand(const char tag[4], const uint8_t* payload, uint16_t len);
+
 // ============================================================================
 // Individual Command Handlers
 // ============================================================================
@@ -80,6 +83,9 @@ void handleVisemeDelete(const uint8_t* payload, uint16_t len);
 void handleVisemeSet(const uint8_t* payload, uint16_t len);
 void handleVisemeTrigger(const uint8_t* payload, uint16_t len);
 
+// Settings
+void handleSettingsSet(const uint8_t* payload, uint16_t len);
+
 // System
 void handleBootloader(const uint8_t* payload, uint16_t len);
 

ls_firmware.ino

@@ -24,6 +24,7 @@
 #include "protocol.h"
 #include "sensors.h"
 #include "behaviors.h"
+#include "websocket_client.h"
 #include <FFat.h>
 
 
@@ -527,6 +528,10 @@ void sendHeartbeat() {
     flags |= 0x08;
   if (sensors.isRadarStreamEnabled())
     flags |= 0x10;
+  if (sensors.isFaceStreamEnabled())
+    flags |= 0x20;
+  if (faceDetect.isAlive())
+    flags |= 0x40;
 
   payload[0] = uptime & 0xFF;
   payload[1] = (uptime >> 8) & 0xFF;
@@ -574,8 +579,7 @@ void setup() {
   // Priority: Focus > Viseme > Idle
   // NOTE: Don't set enabled state here - let config load restore it, or set defaults after
   
-  // 1. Focus behavior (highest priority - radar tracking)
-  static FocusBehavior focusBehavior;
+  // 1. Focus behavior (highest priority - face tracking)
   behaviorManager.addBehavior(BEHAVIOR_FOCUS, &focusBehavior);
   
   // 2. Viseme behavior (medium priority - mouth animation for speech)
@@ -592,7 +596,7 @@ void setup() {
   
   // Load full config with behaviors and visemes (will restore their state)
   // This must happen BEFORE setting defaults, so saved states aren't overwritten
-  bool configLoaded = config.loadOrCreateDefault("/robot_config.bin", &behaviorManager, &visemeBehavior);
+  bool configLoaded = config.loadOrCreateDefault("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
   
   if (configLoaded) {
     Serial.println("[HansonServo] Config loaded: " + config.deviceName);
@@ -605,7 +609,7 @@ void setup() {
       behaviorManager.setBehaviorEnabled(BEHAVIOR_VISEME, true);
       behaviorManager.setBehaviorEnabled(BEHAVIOR_IDLE, true);
       // Save the defaults
-      config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
+      config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
     } else {
       Serial.println("[HansonServo] Behavior states loaded from config");
     }
@@ -626,7 +630,7 @@ void setup() {
       visemeBehavior.addViseme(9, "UH ", 2250, 1850, 2150);  // UH (as in "book")
       visemeBehavior.addViseme(10, "UW ", 2200, 1900, 2100); // UW (as in "boot")
       // Save the defaults
-      config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
+      config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
     } else {
       Serial.println("[HansonServo] Visemes loaded from config: " + String(visemeBehavior.getVisemes().size()));
     }
@@ -638,6 +642,9 @@ void setup() {
     behaviorManager.setBehaviorEnabled(BEHAVIOR_IDLE, true);
   }
   
+  // WebSocket client + server (WiFi must be after config load so credentials are ready)
+  websocketSetup();
+
   // Initialize idle behavior motors (needs config.motors to be loaded)
   std::vector<uint8_t> allMotorIDs;
   for (const Motor& motor : config.motors) {
@@ -702,6 +709,9 @@ void loop() {
   // Protocol handling
   handleProtocol();
 
+  // WebSocket client (receive bytes, FACE packets)
+  websocketLoop();
+
   // Update behaviors
   behaviorManager.update();
 
@@ -723,7 +733,7 @@ void loop() {
   }
   //printMotorStats();
   // Sensor updates and streaming
-  //sensors.update();
+  sensors.update();
 
   // Heartbeat
   //sendHeartbeat();

protocol.h

@@ -44,6 +44,8 @@ namespace Tag {
   // Sensors
   constexpr char IMU[4]   = {'I','M','U','0'};  // ADXL acceleration data (x,y,z)
   constexpr char RADAR[4] = {'R','D','A','R'};  // Radar targets
+  constexpr char FACE[4]  = {'F','A','C','E'};  // Face detection data (via WiFi)
+  constexpr char ALIV[4]  = {'A','L','I','V'};  // Remote component alive/heartbeat
   constexpr char FRAME[4] = {'F','R','M','E'};  // Animation frame events (frame, mode, status)
   
   // Behaviors
@@ -57,6 +59,9 @@ namespace Tag {
   constexpr char VSET[4]  = {'V','S','E','T'};  // Set viseme motor positions
   constexpr char VSME[4]  = {'V','S','M','E'};  // Trigger a viseme by ID
   
+  // Settings
+  constexpr char SSET[4]  = {'S','S','E','T'};  // Settings set/dump (id + value pairs)
+  
   // System
   constexpr char STATE[4] = {'S','T','A','T'};  // System state/heartbeat
   constexpr char MSGE[4]  = {'M','S','G','E'};  // Log/debug message

robotconfig.cpp

@@ -1,5 +1,6 @@
 #include "robotconfig.h"
 #include "behaviors.h"
+#include "websocket_client.h"
 #include <FFat.h>
 
 uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
@@ -227,7 +228,63 @@ bool RobotConfig::loadOrCreateDefault(const char* path) {
 // Key-Value Format V2 (Compact, Extensible)
 // ============================================================================
 
-bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) const {
+// ---- Helpers for reading/writing multi-byte values to file ----
+
+static void writeU16(File& f, uint16_t v) {
+  f.write((uint8_t)(v & 0xFF));
+  f.write((uint8_t)((v >> 8) & 0xFF));
+}
+
+static uint16_t readU16(File& f) {
+  uint16_t v = f.read();
+  v |= ((uint16_t)f.read() << 8);
+  return v;
+}
+
+static void writeU32(File& f, uint32_t v) {
+  f.write((uint8_t)(v & 0xFF));
+  f.write((uint8_t)((v >> 8) & 0xFF));
+  f.write((uint8_t)((v >> 16) & 0xFF));
+  f.write((uint8_t)((v >> 24) & 0xFF));
+}
+
+static uint32_t readU32(File& f) {
+  uint32_t v = f.read();
+  v |= ((uint32_t)f.read() << 8);
+  v |= ((uint32_t)f.read() << 16);
+  v |= ((uint32_t)f.read() << 24);
+  return v;
+}
+
+static void writeFloat(File& f, float v) {
+  uint32_t raw;
+  memcpy(&raw, &v, 4);
+  writeU32(f, raw);
+}
+
+static float readFloat(File& f) {
+  uint32_t raw = readU32(f);
+  float v;
+  memcpy(&v, &raw, 4);
+  return v;
+}
+
+static void writeStr(File& f, const char* s, uint8_t maxLen) {
+  uint8_t len = strlen(s);
+  if (len > maxLen) len = maxLen;
+  f.write(len);
+  f.write((const uint8_t*)s, len);
+}
+
+static void readStr(File& f, char* dst, uint8_t maxLen) {
+  uint8_t len = f.read();
+  if (len > maxLen) len = maxLen;
+  f.readBytes(dst, len);
+  dst[len] = '\0';
+  // Skip extra bytes if stored length exceeded maxLen
+}
+
+bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior, FocusBehavior* focusBehavior) const {
   File file = FFat.open(path, FILE_WRITE);
   if (!file) return false;
 
@@ -343,6 +400,69 @@ bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManage
     }
   }
 
+  // Setting 7: Focus Behavior Settings
+  if (focusBehavior) {
+    const FocusSettings& fs = focusBehavior->getSettings();
+    
+    file.write((uint8_t)(KEY_FOCUS_SETTINGS & 0xFF));
+    file.write((uint8_t)((KEY_FOCUS_SETTINGS >> 8) & 0xFF));
+    file.write(TYPE_FOCUS_SETTINGS);
+    
+    // Motor IDs
+    file.write(fs.eyeMotor1);
+    file.write(fs.eyeMotor2);
+    file.write(fs.neckMotor);
+    
+    // Eye servo range
+    writeU16(file, fs.eyeCenter);
+    writeU16(file, fs.eyeMin);
+    writeU16(file, fs.eyeMax);
+    
+    // Neck servo range
+    writeU16(file, fs.neckCenter);
+    writeU16(file, fs.neckMin);
+    writeU16(file, fs.neckMax);
+    
+    // Face x range
+    writeFloat(file, fs.faceXMin);
+    writeFloat(file, fs.faceXMax);
+    
+    // Interpolation speeds
+    writeFloat(file, fs.eyeSpeed);
+    writeFloat(file, fs.neckSpeed);
+    writeFloat(file, fs.eyeReturnSpeed);
+    
+    // Neck delay
+    writeU32(file, (uint32_t)fs.neckDelayMs);
+    
+    // Neck contribution
+    writeFloat(file, fs.neckContribution);
+    
+    // Neck invert
+    file.write(fs.neckInvert ? 1 : 0);
+    
+    // Centering speeds
+    writeFloat(file, fs.eyeCenteringSpeed);
+    writeFloat(file, fs.neckCenteringSpeed);
+    
+    settingCount++;
+  }
+
+  // Setting 8: WiFi / WebSocket Settings
+  {
+    file.write((uint8_t)(KEY_WIFI_SETTINGS & 0xFF));
+    file.write((uint8_t)((KEY_WIFI_SETTINGS >> 8) & 0xFF));
+    file.write(TYPE_WIFI_SETTINGS);
+    
+    writeStr(file, wifiSettings.ssid, 32);
+    writeStr(file, wifiSettings.password, 64);
+    writeStr(file, wifiSettings.host, 63);
+    writeU16(file, wifiSettings.port);
+    writeStr(file, wifiSettings.path, 31);
+    
+    settingCount++;
+  }
+
   // Write setting count at the beginning
   size_t endPos = file.position();
   file.seek(countPos);
@@ -354,7 +474,7 @@ bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManage
   return true;
 }
 
-bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) {
+bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior, FocusBehavior* focusBehavior) {
   File file = FFat.open(path, FILE_READ);
   if (!file) return false;
 
@@ -479,6 +599,81 @@ bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorMana
         break;
       }
 
+      case KEY_FOCUS_SETTINGS: {
+        if (type == TYPE_FOCUS_SETTINGS && focusBehavior) {
+          FocusSettings& fs = focusBehavior->getSettings();
+          
+          // Motor IDs
+          fs.eyeMotor1 = file.read();
+          fs.eyeMotor2 = file.read();
+          fs.neckMotor = file.read();
+          
+          // Eye servo range
+          fs.eyeCenter = readU16(file);
+          fs.eyeMin = readU16(file);
+          fs.eyeMax = readU16(file);
+          
+          // Neck servo range
+          fs.neckCenter = readU16(file);
+          fs.neckMin = readU16(file);
+          fs.neckMax = readU16(file);
+          
+          // Face x range
+          fs.faceXMin = readFloat(file);
+          fs.faceXMax = readFloat(file);
+          
+          // Interpolation speeds
+          fs.eyeSpeed = readFloat(file);
+          fs.neckSpeed = readFloat(file);
+          fs.eyeReturnSpeed = readFloat(file);
+          
+          // Neck delay
+          fs.neckDelayMs = (unsigned long)readU32(file);
+          
+          // Neck contribution
+          fs.neckContribution = readFloat(file);
+          
+          // Neck invert
+          fs.neckInvert = file.read() != 0;
+          
+          // Centering speeds
+          fs.eyeCenteringSpeed = readFloat(file);
+          fs.neckCenteringSpeed = readFloat(file);
+          
+          Serial.println("[Config] Focus settings loaded");
+        } else {
+          // Skip the focus settings blob (52 bytes) if no focusBehavior
+          for (int k = 0; k < 52; k++) file.read();
+        }
+        break;
+      }
+
+      case KEY_WIFI_SETTINGS: {
+        if (type == TYPE_WIFI_SETTINGS) {
+          // TODO: re-enable when SSET wifi writes are working
+          // For now, skip loading and use hardcoded defaults from WiFiSettings struct
+          // readStr(file, wifiSettings.ssid, 32);
+          // readStr(file, wifiSettings.password, 64);
+          // readStr(file, wifiSettings.host, 63);
+          // wifiSettings.port = readU16(file);
+          // readStr(file, wifiSettings.path, 31);
+          // Serial.println("[Config] WiFi settings loaded");
+          
+          // Skip the stored blob so file position stays correct
+          for (int s = 0; s < 3; s++) { uint8_t l = file.read(); for (uint8_t k = 0; k < l; k++) file.read(); }
+          file.read(); file.read(); // port
+          for (int s = 0; s < 2; s++) { uint8_t l = file.read(); for (uint8_t k = 0; k < l; k++) file.read(); }
+          Serial.println("[Config] WiFi settings skipped (using hardcoded)");
+        } else {
+          // Skip: 5 length-prefixed strings + 2 byte port - can't know exact size
+          // Best effort: skip based on stored lengths
+          for (int s = 0; s < 3; s++) { uint8_t l = file.read(); for (uint8_t k = 0; k < l; k++) file.read(); }
+          file.read(); file.read(); // port
+          for (int s = 0; s < 2; s++) { uint8_t l = file.read(); for (uint8_t k = 0; k < l; k++) file.read(); }
+        }
+        break;
+      }
+
       default:
         // Unknown key - skip based on type
         switch (type) {
@@ -507,11 +702,11 @@ bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorMana
   return true;
 }
 
-bool RobotConfig::loadOrCreateDefault(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) {
+bool RobotConfig::loadOrCreateDefault(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior, FocusBehavior* focusBehavior) {
   if (FFat.exists(path)) {
     Serial.println("Loading robot config from FFat...");
     // Try V2 format first
-    if (loadFromFFatV2(path, behaviorManager, visemeBehavior)) {
+    if (loadFromFFatV2(path, behaviorManager, visemeBehavior, focusBehavior)) {
       Serial.println("Loaded V2 format");
       return true;
     }
@@ -519,17 +714,17 @@ bool RobotConfig::loadOrCreateDefault(const char* path, BehaviorManager* behavio
     if (loadFromFFat(path)) {
       Serial.println("Loaded V1 format (legacy)");
       // Upgrade to V2 format
-      saveToFFatV2(path, behaviorManager, visemeBehavior);
+      saveToFFatV2(path, behaviorManager, visemeBehavior, focusBehavior);
       return true;
     }
   }
 
   Serial.println("No config found. Creating default config...");
 
-  // 🔧 Define your default config here
+  // Define your default config here
   deviceName = "DefaultBot";
   firmwareVersion = { 1, 0 };
   motors.clear();
 
-  return saveToFFatV2(path, behaviorManager, visemeBehavior);
+  return saveToFFatV2(path, behaviorManager, visemeBehavior, focusBehavior);
 }

robotconfig.h

@@ -5,6 +5,7 @@
 // Forward declarations
 class BehaviorManager;
 class VisemeBehavior;
+class FocusBehavior;
 
 // ============================================================================
 // Config Key-Value System
@@ -25,6 +26,12 @@ enum ConfigKey : uint16_t {
   // Viseme array (single entry containing all visemes)
   KEY_VISEME_ARRAY = 0x0300,
   
+  // Focus behavior settings
+  KEY_FOCUS_SETTINGS = 0x0500,
+  
+  // WiFi / WebSocket settings
+  KEY_WIFI_SETTINGS = 0x0600,
+  
   // Future extensible settings
   KEY_SERIAL_BAUD = 0x0400,
   KEY_MOTOR_UPDATE_INTERVAL = 0x0401,
@@ -44,6 +51,8 @@ enum ConfigType : uint8_t {
   TYPE_MOTOR_ARRAY = 0x0A,      // Special type for motor array
   TYPE_BEHAVIOR_STATES = 0x0B,   // Special type for behavior state array
   TYPE_VISEME_ARRAY = 0x0C,      // Special type for viseme array
+  TYPE_FOCUS_SETTINGS = 0x0D,    // Focus behavior settings blob
+  TYPE_WIFI_SETTINGS = 0x0E,     // WiFi/WebSocket settings blob
 };
 
 struct FirmwareVersion {
@@ -88,15 +97,18 @@ struct RobotConfig {
   // New key-value format (v2)
   bool saveToFFatV2(const char* path = "/robot_config.bin", 
                     BehaviorManager* behaviorManager = nullptr,
-                    VisemeBehavior* visemeBehavior = nullptr) const;
+                    VisemeBehavior* visemeBehavior = nullptr,
+                    FocusBehavior* focusBehavior = nullptr) const;
   bool loadFromFFatV2(const char* path = "/robot_config.bin",
                       BehaviorManager* behaviorManager = nullptr,
-                      VisemeBehavior* visemeBehavior = nullptr);
+                      VisemeBehavior* visemeBehavior = nullptr,
+                      FocusBehavior* focusBehavior = nullptr);
   
   // New version with behavior/viseme support
   bool loadOrCreateDefault(const char* path = "/robot_config.bin",
                            BehaviorManager* behaviorManager = nullptr,
-                           VisemeBehavior* visemeBehavior = nullptr);
+                           VisemeBehavior* visemeBehavior = nullptr,
+                           FocusBehavior* focusBehavior = nullptr);
   
   // Legacy version (for backward compatibility)
   bool loadOrCreateDefault(const char* path);

sensors.cpp

@@ -7,6 +7,7 @@
 
 Radar radar;
 ADXL345 adxl;
+FaceDetect faceDetect;
 SensorManager sensors;
 
 // ============================================================================
@@ -277,6 +278,69 @@ void ADXL345::readAccelData() {
   accelZ = z_raw * scale;
 }
 
+// ============================================================================
+// Face Detection Implementation
+// ============================================================================
+
+static DetectedFace s_emptyFace = {0, 0, 0, 0, 0, false};
+
+void FaceDetect::feedPayload(const uint8_t* payload, size_t len) {
+  // FACE payload: [face_count:1][x:2s][y:2s][w:2][h:2][conf:1] per face
+  if (len < 1) return;
+  
+  faceCount = payload[0];
+  if (faceCount > FACE_MAX_FACES) faceCount = FACE_MAX_FACES;
+  
+  size_t offset = 1;
+  for (uint8_t i = 0; i < faceCount; i++) {
+    if (offset + 9 > len) {
+      // Truncated data
+      faces[i].valid = false;
+      continue;
+    }
+    faces[i].x    = (int16_t)(payload[offset] | (payload[offset + 1] << 8));
+    faces[i].y    = (int16_t)(payload[offset + 2] | (payload[offset + 3] << 8));
+    faces[i].w    = payload[offset + 4] | (payload[offset + 5] << 8);
+    faces[i].h    = payload[offset + 6] | (payload[offset + 7] << 8);
+    faces[i].conf = payload[offset + 8];
+    faces[i].valid = true;
+    offset += 9;
+  }
+  
+  // Invalidate remaining slots
+  for (uint8_t i = faceCount; i < FACE_MAX_FACES; i++) {
+    faces[i].valid = false;
+  }
+  
+  newData = true;
+}
+
+const DetectedFace& FaceDetect::getFace(uint8_t index) const {
+  if (index >= FACE_MAX_FACES) return s_emptyFace;
+  return faces[index];
+}
+
+uint16_t FaceDetect::packPayload(uint8_t* buffer) const {
+  // Same format as FACE protocol: [face_count:1][x:2s][y:2s][w:2][h:2][conf:1] per face
+  uint16_t offset = 0;
+  buffer[offset++] = faceCount;
+  
+  for (uint8_t i = 0; i < faceCount; i++) {
+    const DetectedFace& f = faces[i];
+    buffer[offset++] = f.x & 0xFF;
+    buffer[offset++] = (f.x >> 8) & 0xFF;
+    buffer[offset++] = f.y & 0xFF;
+    buffer[offset++] = (f.y >> 8) & 0xFF;
+    buffer[offset++] = f.w & 0xFF;
+    buffer[offset++] = (f.w >> 8) & 0xFF;
+    buffer[offset++] = f.h & 0xFF;
+    buffer[offset++] = (f.h >> 8) & 0xFF;
+    buffer[offset++] = f.conf;
+  }
+  
+  return offset;
+}
+
 // ============================================================================
 // Sensor Manager Implementation
 // ============================================================================
@@ -312,6 +376,12 @@ void SensorManager::update() {
     sendRadarPacket();
     lastRadarSend = now;
   }
+
+  if (faceStreamEnabled && faceDetect.hasNewData() && (now - lastFaceSend >= faceInterval)) {
+    sendFacePacket();
+    faceDetect.clearNewData();
+    lastFaceSend = now;
+  }
 }
 
 void SensorManager::enableADXLStream(bool enable, uint16_t intervalMs) {
@@ -326,6 +396,12 @@ void SensorManager::enableRadarStream(bool enable, uint16_t intervalMs) {
   lastRadarSend = millis();
 }
 
+void SensorManager::enableFaceStream(bool enable, uint16_t intervalMs) {
+  faceStreamEnabled = enable;
+  faceInterval = intervalMs;
+  lastFaceSend = millis();
+}
+
 void SensorManager::sendADXLPacket() {
   uint8_t payload[32];  // Buffer sized for current/future payload expansion
   uint16_t len = adxl.packPayload(payload);
@@ -338,3 +414,8 @@ void SensorManager::sendRadarPacket() {
   sendPacket(Tag::RADAR, payload, len);
 }
 
+void SensorManager::sendFacePacket() {
+  uint8_t payload[64];  // 1 + (9 * FACE_MAX_FACES)
+  uint16_t len = faceDetect.packPayload(payload);
+  sendPacket(Tag::FACE, payload, len);
+}

sensors.h

@@ -91,12 +91,53 @@ private:
   void readAccelData();
 };
 
+// ============================================================================
+// Face Detection - via WebSocket from Radxa
+// ============================================================================
+
+constexpr int FACE_MAX_FACES = 5;
+
+struct DetectedFace {
+  int16_t x;        // Center-relative pixels (0,0 = image center)
+  int16_t y;        // Positive = right/down, negative = left/up
+  uint16_t w;       // Bounding box width
+  uint16_t h;       // Bounding box height
+  uint8_t conf;     // Confidence 0-255
+  bool valid;
+};
+
+class FaceDetect {
+public:
+  // Feed raw FACE payload (after 4-byte tag) from WebSocket
+  void feedPayload(const uint8_t* payload, size_t len);
+  
+  const DetectedFace& getFace(uint8_t index) const;
+  uint8_t getFaceCount() const { return faceCount; }
+  bool hasNewData() const { return newData; }
+  void clearNewData() { newData = false; }
+  
+  // Remote alive state
+  void setAlive(bool alive) { remoteAlive = alive; lastAliveTime = millis(); }
+  bool isAlive() const { return remoteAlive && (millis() - lastAliveTime < 5000); }
+  
+  // Pack faces into payload buffer, returns length
+  uint16_t packPayload(uint8_t* buffer) const;
+  
+private:
+  DetectedFace faces[FACE_MAX_FACES] = {};
+  uint8_t faceCount = 0;
+  bool newData = false;
+  bool remoteAlive = false;
+  unsigned long lastAliveTime = 0;
+};
+
 // ============================================================================
 // Global Instances
 // ============================================================================
 
 extern Radar radar;
 extern ADXL345 adxl;
+extern FaceDetect faceDetect;
 
 // ============================================================================
 // Sensor Manager
@@ -110,20 +151,26 @@ public:
   // Streaming control
   void enableADXLStream(bool enable, uint16_t intervalMs = 100);
   void enableRadarStream(bool enable, uint16_t intervalMs = 100);
+  void enableFaceStream(bool enable, uint16_t intervalMs = 100);
 
   bool isADXLStreamEnabled() const { return adxlStreamEnabled; }
   bool isRadarStreamEnabled() const { return radarStreamEnabled; }
+  bool isFaceStreamEnabled() const { return faceStreamEnabled; }
 
 private:
   bool adxlStreamEnabled = true;
   bool radarStreamEnabled = true;
+  bool faceStreamEnabled = true;
   uint16_t adxlInterval = 500;  // 500ms = 2 packets per second
   uint16_t radarInterval = 10;
+  uint16_t faceInterval = 50;   // 20 Hz
   unsigned long lastADXLSend = 0;
   unsigned long lastRadarSend = 0;
+  unsigned long lastFaceSend = 0;
 
   void sendADXLPacket();
   void sendRadarPacket();
+  void sendFacePacket();
 };
 
 extern SensorManager sensors;

tools/viseme_sender.py

@@ -0,0 +1,232 @@
+"""
+Viseme Sender - sends viseme packets to the robot over WebSocket.
+
+Usage:
+  python viseme_sender.py [robot_ip]
+
+Default IP: 192.168.1.x (auto-discovered or specify as argument)
+Connects to ws://<ip>:81
+
+Controls:
+  0-9, A      = send viseme 0-10
+  Space       = send neutral (viseme 0 / sil)
+  Q / Esc     = quit
+  L           = request viseme list (VLST)
+"""
+
+import asyncio
+import struct
+import sys
+
+try:
+    import websockets
+except ImportError:
+    print("Missing dependency. Install with:  pip install websockets")
+    sys.exit(1)
+
+# ---------------------------------------------------------------------------
+# Protocol helpers
+# ---------------------------------------------------------------------------
+
+SYNC = b'\xA5\x5A'
+
+def crc16_ccitt(data: bytes, init: int = 0xFFFF) -> int:
+    crc = init
+    for b in data:
+        crc ^= b << 8
+        for _ in range(8):
+            if crc & 0x8000:
+                crc = (crc << 1) ^ 0x1021
+            else:
+                crc <<= 1
+            crc &= 0xFFFF
+    return crc
+
+_seq = 0
+
+def build_packet(tag: str, payload: bytes = b'') -> bytes:
+    global _seq
+    tag_bytes = tag.encode('ascii')[:4].ljust(4, b'\x00')
+    length = len(payload)
+    header_tail = tag_bytes + struct.pack('<HH', length, _seq)
+    crc_data = header_tail + payload
+    crc = crc16_ccitt(crc_data)
+    packet = SYNC + crc_data + struct.pack('<H', crc)
+    _seq = (_seq + 1) & 0xFFFF
+    return packet
+
+def parse_packet(data: bytes):
+    """Parse a protocol packet, return (tag, payload) or None."""
+    if len(data) < 12:
+        return None
+    if data[0:2] != SYNC:
+        return None
+    tag = data[2:6].decode('ascii', errors='replace')
+    length = struct.unpack('<H', data[6:8])[0]
+    # seq = struct.unpack('<H', data[8:10])[0]
+    if len(data) < 10 + length + 2:
+        return None
+    payload = data[10:10 + length]
+    received_crc = struct.unpack('<H', data[10 + length:12 + length])[0]
+    computed_crc = crc16_ccitt(data[2:10 + length])
+    if received_crc != computed_crc:
+        return None
+    return tag, payload
+
+# ---------------------------------------------------------------------------
+# Viseme table
+# ---------------------------------------------------------------------------
+
+VISEMES = {
+    0:  'sil',
+    1:  'AA',
+    2:  'AE',
+    3:  'AH',
+    4:  'AO',
+    5:  'EH',
+    6:  'IH',
+    7:  'IY',
+    8:  'OW',
+    9:  'UH',
+    10: 'UW',
+}
+
+KEY_MAP = {
+    '0': 0, '1': 1, '2': 2, '3': 3, '4': 4,
+    '5': 5, '6': 6, '7': 7, '8': 8, '9': 9,
+    'a': 10, 'b': 11, 'c': 12, 'd': 13, 'e': 14,
+    'f': 15, 'g': 16, 'h': 17, 'i': 18, 'j': 19,
+    ' ': 0,
+}
+
+# ---------------------------------------------------------------------------
+# Main
+# ---------------------------------------------------------------------------
+
+async def main():
+    ip = sys.argv[1] if len(sys.argv) > 1 else '192.168.1.1'
+    uri = f'ws://{ip}:81'
+
+    print(f'Connecting to {uri} ...')
+    try:
+        ws = await websockets.connect(uri)
+    except Exception as e:
+        print(f'Connection failed: {e}')
+        return
+
+    print(f'Connected to {uri}')
+    print()
+    print('Viseme keys:')
+    for k, vid in sorted(KEY_MAP.items()):
+        label = VISEMES.get(vid, f'viseme {vid}')
+        key_label = 'Space' if k == ' ' else k.upper()
+        print(f'  [{key_label}]  ->  {vid}: {label}')
+    print('  [L]    ->  list visemes from device')
+    print('  [Q/Esc] -> quit')
+    print()
+
+    # Background task to receive and print responses
+    async def receiver():
+        try:
+            async for message in ws:
+                if isinstance(message, bytes):
+                    result = parse_packet(message)
+                    if result:
+                        tag, payload = result
+                        if tag == 'VLST':
+                            print_viseme_list(payload)
+                        elif tag == 'ACK!':
+                            print(f'  <- ACK')
+                        elif tag.startswith('NAC'):
+                            msg = payload.decode('ascii', errors='replace') if payload else ''
+                            print(f'  <- NACK: {msg}')
+                        elif tag == 'MSGE':
+                            msg = payload.decode('ascii', errors='replace')
+                            print(f'  <- MSG: {msg}')
+                        else:
+                            print(f'  <- [{tag}] {len(payload)} bytes')
+        except websockets.exceptions.ConnectionClosed:
+            pass
+
+    recv_task = asyncio.create_task(receiver())
+
+    # Input loop (runs in executor to avoid blocking)
+    loop = asyncio.get_event_loop()
+    try:
+        while True:
+            key = await loop.run_in_executor(None, get_key)
+            if key is None or key in ('q', '\x1b'):
+                break
+            if key == 'l':
+                pkt = build_packet('VLST')
+                await ws.send(pkt)
+                print('  -> VLST (list visemes)')
+                continue
+            if key in KEY_MAP:
+                vid = KEY_MAP[key]
+                pkt = build_packet('VSME', bytes([vid]))
+                await ws.send(pkt)
+                label = VISEMES.get(vid, f'viseme {vid}')
+                print(f'  -> VSME {vid}: {label}')
+    except (KeyboardInterrupt, EOFError):
+        pass
+    finally:
+        recv_task.cancel()
+        await ws.close()
+        print('Disconnected.')
+
+
+def print_viseme_list(payload: bytes):
+    if len(payload) < 1:
+        print('  <- VLST: (empty)')
+        return
+    count = payload[0]
+    print(f'  <- VLST: {count} visemes')
+    pos = 1
+    for _ in range(count):
+        if pos + 4 >= len(payload):
+            break
+        vid = payload[pos]
+        label = payload[pos+1:pos+4].decode('ascii', errors='replace')
+        motor_count = payload[pos+4]
+        pos += 5
+        motors = []
+        for _ in range(motor_count):
+            if pos + 3 > len(payload):
+                break
+            mid = payload[pos]
+            mpos = struct.unpack('<H', payload[pos+1:pos+3])[0]
+            motors.append(f'{mid}={mpos}')
+            pos += 3
+        print(f'     [{vid:2d}] "{label}" motors: {", ".join(motors) if motors else "(none)"}')
+
+
+# ---------------------------------------------------------------------------
+# Cross-platform single key input
+# ---------------------------------------------------------------------------
+
+def get_key() -> str | None:
+    """Block until a single key is pressed, return it lowercase."""
+    try:
+        import msvcrt  # Windows
+        ch = msvcrt.getwch()
+        return ch.lower()
+    except ImportError:
+        pass
+    # Unix / macOS
+    import tty, termios
+    fd = sys.stdin.fileno()
+    old = termios.tcgetattr(fd)
+    try:
+        tty.setraw(fd)
+        ch = sys.stdin.read(1)
+        return ch.lower()
+    finally:
+        termios.tcsetattr(fd, termios.TCSADRAIN, old)
+
+
+if __name__ == '__main__':
+    try:
+        asyncio.run(main())
+    except KeyboardInterrupt:
+        print('\nBye.')

websocket_client.cpp

@@ -0,0 +1,277 @@
+#include "websocket_client.h"
+#include "sensors.h"
+#include "protocol.h"
+#include "commands.h"
+#include <ArduinoWebsockets.h>
+#include <WiFi.h>
+
+using namespace websockets;
+
+WiFiSettings wifiSettings;  // Global runtime instance
+
+// ============================================================================
+// Outbound client (connects to Radxa for FACE/ALIV)
+// ============================================================================
+
+static WebsocketsClient client;
+static bool s_connected = false;
+static unsigned long lastReconnectAttempt = 0;
+constexpr unsigned long RECONNECT_INTERVAL = 5000;
+
+static String buildWsUrl() {
+  return "ws://" + String(wifiSettings.host) + ":" + String(wifiSettings.port) + String(wifiSettings.path);
+}
+
+// ============================================================================
+// Inbound server (accepts commands from computer)
+// ============================================================================
+
+static WebsocketsServer* server = nullptr;
+constexpr uint8_t MAX_SERVER_CLIENTS = 2;
+static WebsocketsClient serverClients[MAX_SERVER_CLIENTS];
+static bool serverClientActive[MAX_SERVER_CLIENTS] = {false};
+
+// ============================================================================
+// Shared protocol packet parser
+// ============================================================================
+
+// Parse protocol-framed binary data, call handler for each valid packet
+static void parseProtocolPackets(const uint8_t* data, size_t len,
+    void (*handler)(const char tag[4], const uint8_t* payload, uint16_t payloadLen))
+{
+  size_t pos = 0;
+  
+  while (pos + 12 <= len) {
+    if (data[pos] != 0xA5 || data[pos + 1] != 0x5A) {
+      pos++;
+      continue;
+    }
+    
+    char tag[4];
+    memcpy(tag, &data[pos + 2], 4);
+    
+    uint16_t payloadLen = data[pos + 6] | (data[pos + 7] << 8);
+    
+    size_t totalPacketLen = 2 + 4 + 2 + 2 + payloadLen + 2;
+    if (pos + totalPacketLen > len) break;
+    
+    const uint8_t* payload = &data[pos + 10];
+    
+    // Verify CRC
+    const uint8_t* crcData = &data[pos + 2];
+    uint16_t crcDataLen = 4 + 2 + 2 + payloadLen;
+    uint16_t computed = crc16Compute(crcData, crcDataLen);
+    uint16_t received = data[pos + 10 + payloadLen] | (data[pos + 10 + payloadLen + 1] << 8);
+    
+    if (computed == received) {
+      handler(tag, payload, payloadLen);
+    }
+    
+    pos += totalPacketLen;
+  }
+}
+
+// ============================================================================
+// Client packet handler (FACE/ALIV from Radxa)
+// ============================================================================
+
+static void handleClientPacket(const char tag[4], const uint8_t* payload, uint16_t len) {
+  if (memcmp(tag, Tag::FACE, 4) == 0) {
+    faceDetect.feedPayload(payload, len);
+  }
+  else if (memcmp(tag, Tag::ALIV, 4) == 0) {
+    if (len >= 2) {
+      uint8_t componentId = payload[0];
+      uint8_t alive = payload[1];
+      if (componentId == 3) {
+        faceDetect.setAlive(alive != 0);
+      }
+    }
+    sendPacket(Tag::ALIV, payload, len);
+  }
+}
+
+// ============================================================================
+// Server packet handler (any command from computer)
+// ============================================================================
+
+static void handleServerPacket(const char tag[4], const uint8_t* payload, uint16_t len) {
+  // Route to the standard command dispatcher - handles VSME, MSET, BHVR, SSET, etc.
+  dispatchCommand(tag, payload, len);
+}
+
+// ============================================================================
+// Client callbacks
+// ============================================================================
+
+static void onClientMessage(WebsocketsMessage message) {
+  std::string raw = message.rawData();
+  if (raw.empty()) return;
+  parseProtocolPackets(reinterpret_cast<const uint8_t*>(raw.data()), raw.size(), handleClientPacket);
+}
+
+static void onClientEvent(WebsocketsEvent event, String data) {
+  switch (event) {
+    case WebsocketsEvent::ConnectionOpened:
+      s_connected = true;
+      Serial.println("[WS Client] Connected");
+      break;
+    case WebsocketsEvent::ConnectionClosed:
+      s_connected = false;
+      Serial.println("[WS Client] Disconnected");
+      break;
+    default: break;
+  }
+}
+
+// ============================================================================
+// Server client callbacks (created per-client via lambda in loop)
+// ============================================================================
+
+static void onServerMessage(WebsocketsMessage message) {
+  std::string raw = message.rawData();
+  if (raw.empty()) return;
+  parseProtocolPackets(reinterpret_cast<const uint8_t*>(raw.data()), raw.size(), handleServerPacket);
+}
+
+static void onServerEvent(WebsocketsEvent event, String data) {
+  // Connection events are handled in the accept logic
+}
+
+// ============================================================================
+// Public API
+// ============================================================================
+
+void websocketSetup() {
+  Serial.print("[WiFi] SSID: '");
+  Serial.print(wifiSettings.ssid);
+  Serial.println("'");
+  
+  WiFi.mode(WIFI_STA);
+  WiFi.begin(wifiSettings.ssid, wifiSettings.password);
+  Serial.print("[WiFi] Connecting");
+  int attempts = 0;
+  while (WiFi.status() != WL_CONNECTED && attempts < 30) {
+    delay(500);
+    Serial.print(".");
+    attempts++;
+  }
+  Serial.println();
+  if (WiFi.status() != WL_CONNECTED) {
+    Serial.println("[WiFi] Failed to connect");
+    return;
+  }
+  Serial.print("[WiFi] OK ");
+  Serial.println(WiFi.localIP());
+
+  // --- Start outbound client (to Radxa) ---
+  client.onMessage(onClientMessage);
+  client.onEvent(onClientEvent);
+
+  String url = buildWsUrl();
+  Serial.println("[WS Client] Connecting to " + url);
+  if (client.connect(url)) {
+    s_connected = true;
+    Serial.println("[WS Client] Connected");
+  } else {
+    Serial.println("[WS Client] Connect failed (will retry)");
+  }
+
+  // --- Start inbound server (allocated after WiFi to avoid early init issues) ---
+  server = new WebsocketsServer();
+  server->listen(WS_SERVER_PORT);
+  if (server->available()) {
+    Serial.print("[WS Server] Listening on port ");
+    Serial.println(WS_SERVER_PORT);
+  } else {
+    Serial.println("[WS Server] Failed to start");
+  }
+}
+
+void websocketLoop() {
+  if (WiFi.status() != WL_CONNECTED) {
+    s_connected = false;
+    return;
+  }
+
+  // --- Outbound client ---
+  if (!s_connected && !client.available()) {
+    unsigned long now = millis();
+    if (now - lastReconnectAttempt >= RECONNECT_INTERVAL) {
+      lastReconnectAttempt = now;
+      String url = buildWsUrl();
+      if (client.connect(url)) {
+        s_connected = true;
+        Serial.println("[WS Client] Reconnected");
+      }
+    }
+  }
+
+  if (client.available()) {
+    client.poll();
+  }
+
+  // --- Inbound server: accept new connections ---
+  if (server && server->available() && server->poll()) {
+    WebsocketsClient newClient = server->accept();
+    if (newClient.available()) {
+      // Find an empty slot
+      bool accepted = false;
+      for (uint8_t i = 0; i < MAX_SERVER_CLIENTS; i++) {
+        if (!serverClientActive[i] || !serverClients[i].available()) {
+          serverClients[i] = std::move(newClient);
+          serverClients[i].onMessage(onServerMessage);
+          serverClients[i].onEvent(onServerEvent);
+          serverClientActive[i] = true;
+          Serial.print("[WS Server] Client connected (slot ");
+          Serial.print(i);
+          Serial.println(")");
+          accepted = true;
+          break;
+        }
+      }
+      if (!accepted) {
+        Serial.println("[WS Server] Max clients reached, rejecting");
+        newClient.close();
+      }
+    }
+  }
+
+  // --- Inbound server: poll existing clients ---
+  for (uint8_t i = 0; i < MAX_SERVER_CLIENTS; i++) {
+    if (serverClientActive[i]) {
+      if (serverClients[i].available()) {
+        serverClients[i].poll();
+      } else {
+        serverClientActive[i] = false;
+        Serial.print("[WS Server] Client disconnected (slot ");
+        Serial.print(i);
+        Serial.println(")");
+      }
+    }
+  }
+}
+
+bool websocketConnected() {
+  return s_connected && client.available();
+}
+
+void websocketReconnect() {
+  client.close();
+  s_connected = false;
+  
+  WiFi.disconnect();
+  WiFi.begin(wifiSettings.ssid, wifiSettings.password);
+  Serial.print("[WiFi] Reconnecting to ");
+  Serial.println(wifiSettings.ssid);
+  
+  lastReconnectAttempt = 0;
+}
+
+uint8_t websocketServerClientCount() {
+  uint8_t count = 0;
+  for (uint8_t i = 0; i < MAX_SERVER_CLIENTS; i++) {
+    if (serverClientActive[i] && serverClients[i].available()) count++;
+  }
+  return count;
+}

websocket_client.h

@@ -0,0 +1,35 @@
+#pragma once
+#include <Arduino.h>
+
+// WebSocket client + server
+// Client: connects out to Radxa to receive FACE/ALIV packets
+// Server: listens for incoming connections to accept any protocol command (VSME, MSET, etc.)
+
+// Runtime-configurable WiFi + WebSocket settings (persisted to NVM)
+struct WiFiSettings {
+  char ssid[33]     = "Police Surveillance Van";
+  char password[65] = "ourpassword";
+  char host[64]     = "192.168.1.206";
+  uint16_t port     = 5001;
+  char path[32]     = "/";
+};
+
+extern WiFiSettings wifiSettings;
+
+// WebSocket server port (for incoming commands from computer)
+constexpr uint16_t WS_SERVER_PORT = 81;
+
+// Call once from setup() after Serial is ready
+void websocketSetup();
+
+// Call every loop() - handles client + server
+void websocketLoop();
+
+// True when outbound client is connected
+bool websocketConnected();
+
+// Force client reconnect (call after settings change)
+void websocketReconnect();
+
+// Number of server clients currently connected
+uint8_t websocketServerClientCount();