implemented websocket data, just face tracking now. focus behaviour uses face tracking instead of radar
parent
b35a384974
commit
cdd7a39e01
|
|
@ -112,6 +112,8 @@ 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) |
|
||||
| `ACK!` | Acknowledge (success) |
|
||||
| `NACK` | Negative acknowledge (failure) |
|
||||
| `BOOT` | Enter bootloader |
|
||||
|
|
@ -277,6 +279,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 +325,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)
|
||||
|
||||
|
|
@ -400,6 +435,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*
|
||||
|
||||
|
|
|
|||
206
behaviors.cpp
206
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);
|
||||
|
||||
// Eyes track immediately
|
||||
targetEyePosition = targetEyePos;
|
||||
// Track when we first saw a face (for neck delay)
|
||||
if (!faceWasPresent) {
|
||||
faceDetectedTime = now;
|
||||
faceWasPresent = true;
|
||||
}
|
||||
|
||||
// Neck disabled for now - keep it centered
|
||||
targetNeckPosition = NECK_POSITION_CENTER;
|
||||
neckRotating = false;
|
||||
// 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;
|
||||
|
||||
// Smoothly interpolate eye position toward target
|
||||
eyePosition = lerp(eyePosition, targetEyePosition, INTERPOLATION_SPEED);
|
||||
// ---- Neck: smoothly follow the target after a delay ----
|
||||
float neckTarget = faceNorm * settings.neckContribution;
|
||||
|
||||
// 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)
|
||||
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);
|
||||
|
||||
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);
|
||||
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));
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
|
|
|
|||
104
behaviors.h
104
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,87 @@ protected:
|
|||
};
|
||||
|
||||
// ============================================================================
|
||||
// Focus Behavior - Tracks radar targets with eyes/neck
|
||||
// Focus Behavior - Tracks faces with eyes/neck via FaceDetect sensor
|
||||
// ============================================================================
|
||||
|
||||
// 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
|
||||
// Access settings for external tuning
|
||||
FocusSettings& getSettings() { return settings; }
|
||||
const FocusSettings& getSettings() const { return settings; }
|
||||
|
||||
// Target positions
|
||||
uint16_t targetEyePosition;
|
||||
uint16_t targetNeckPosition;
|
||||
private:
|
||||
FocusSettings settings;
|
||||
|
||||
bool isActive;
|
||||
|
||||
// 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 targetDetectedTime; // When target was first detected
|
||||
unsigned long neckStartTime; // When neck rotation should start
|
||||
bool neckRotating; // Whether neck is actively rotating
|
||||
unsigned long faceDetectedTime; // When face was first seen (for neck delay)
|
||||
bool faceWasPresent; // Was a face present last frame?
|
||||
|
||||
// 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
|
||||
// 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;
|
||||
|
||||
// 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
|
||||
|
||||
// Calculate motor position from radar angle (in degrees)
|
||||
uint16_t calculateEyePositionFromRadarAngle(float radarAngle);
|
||||
uint16_t calculateNeckPositionFromRadarAngle(float radarAngle);
|
||||
|
||||
// Smooth interpolation helper (linear interpolation)
|
||||
uint16_t lerp(uint16_t current, uint16_t target, float t);
|
||||
// Smooth interpolation helpers
|
||||
static float lerpf(float current, float target, float t);
|
||||
static uint16_t lerp(uint16_t current, uint16_t target, float t);
|
||||
};
|
||||
|
||||
// ============================================================================
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
@ -554,6 +559,9 @@ void setup() {
|
|||
delay(500);
|
||||
Serial.println("\n[HansonServo] Starting...");
|
||||
|
||||
// WebSocket client (WiFi + connect to remote, receive FACE packets)
|
||||
websocketSetup();
|
||||
|
||||
// Initialize servo manager
|
||||
servoManager.init();
|
||||
Serial.println("[HansonServo] Servos initialized");
|
||||
|
|
@ -702,6 +710,9 @@ void loop() {
|
|||
// Protocol handling
|
||||
handleProtocol();
|
||||
|
||||
// WebSocket client (receive bytes, FACE packets)
|
||||
websocketLoop();
|
||||
|
||||
// Update behaviors
|
||||
behaviorManager.update();
|
||||
|
||||
|
|
@ -723,7 +734,7 @@ void loop() {
|
|||
}
|
||||
//printMotorStats();
|
||||
// Sensor updates and streaming
|
||||
//sensors.update();
|
||||
sensors.update();
|
||||
|
||||
// Heartbeat
|
||||
//sendHeartbeat();
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
82
sensors.cpp
82
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,9 @@ 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);
|
||||
}
|
||||
|
||||
|
|
|
|||
47
sensors.h
47
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;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,177 @@
|
|||
#include "websocket_client.h"
|
||||
#include "sensors.h"
|
||||
#include "protocol.h"
|
||||
#include <ArduinoWebsockets.h>
|
||||
#include <WiFi.h>
|
||||
|
||||
using namespace websockets;
|
||||
|
||||
static WebsocketsClient client;
|
||||
static bool s_connected = false;
|
||||
static unsigned long lastReconnectAttempt = 0;
|
||||
constexpr unsigned long RECONNECT_INTERVAL = 5000;
|
||||
|
||||
// ============================================================================
|
||||
// Packet parsing for WebSocket binary messages
|
||||
// Uses the same protocol format: 0xA5 0x5A TAG(4) LEN(2) SEQ(2) PAYLOAD(N) CRC(2)
|
||||
// ============================================================================
|
||||
|
||||
static void processPacketPayload(const char tag[4], const uint8_t* payload, uint16_t len) {
|
||||
if (memcmp(tag, Tag::FACE, 4) == 0) {
|
||||
// Face detection data - feed to FaceDetect sensor
|
||||
// SensorManager will send the FACE packet over serial
|
||||
faceDetect.feedPayload(payload, len);
|
||||
}
|
||||
else if (memcmp(tag, Tag::ALIV, 4) == 0) {
|
||||
// ALIV payload: [component_id:1][alive:1]
|
||||
if (len >= 2) {
|
||||
uint8_t componentId = payload[0];
|
||||
uint8_t alive = payload[1];
|
||||
|
||||
// Component 3 = Face detection (Radxa)
|
||||
if (componentId == 3) {
|
||||
faceDetect.setAlive(alive != 0);
|
||||
}
|
||||
}
|
||||
// Forward ALIV as a proper protocol packet over serial
|
||||
sendPacket(Tag::ALIV, payload, len);
|
||||
}
|
||||
}
|
||||
|
||||
static void parseProtocolMessage(const uint8_t* data, size_t len) {
|
||||
// Walk through the message looking for protocol packets
|
||||
// Format: SYNC0(0xA5) SYNC1(0x5A) TAG(4) LEN(2) SEQ(2) PAYLOAD(N) CRC(2)
|
||||
size_t pos = 0;
|
||||
|
||||
while (pos + 12 <= len) { // Minimum packet: 2 sync + 4 tag + 2 len + 2 seq + 0 payload + 2 crc = 12
|
||||
// Look for sync bytes
|
||||
if (data[pos] != 0xA5 || data[pos + 1] != 0x5A) {
|
||||
pos++;
|
||||
continue;
|
||||
}
|
||||
|
||||
// Read tag
|
||||
char tag[4];
|
||||
memcpy(tag, &data[pos + 2], 4);
|
||||
|
||||
// Read length (LE)
|
||||
uint16_t payloadLen = data[pos + 6] | (data[pos + 7] << 8);
|
||||
|
||||
// Sanity check
|
||||
size_t totalPacketLen = 2 + 4 + 2 + 2 + payloadLen + 2; // sync + tag + len + seq + payload + crc
|
||||
if (pos + totalPacketLen > len) {
|
||||
// Incomplete packet
|
||||
break;
|
||||
}
|
||||
|
||||
// Payload starts after sync(2) + tag(4) + len(2) + seq(2) = offset 10
|
||||
const uint8_t* payload = &data[pos + 10];
|
||||
|
||||
// Verify CRC over tag + len + seq + payload
|
||||
const uint8_t* crcData = &data[pos + 2]; // starts at tag
|
||||
uint16_t crcDataLen = 4 + 2 + 2 + payloadLen; // tag + len + seq + payload
|
||||
uint16_t computed = crc16Compute(crcData, crcDataLen);
|
||||
|
||||
uint16_t received = data[pos + 10 + payloadLen] | (data[pos + 10 + payloadLen + 1] << 8);
|
||||
|
||||
if (computed == received) {
|
||||
processPacketPayload(tag, payload, payloadLen);
|
||||
}
|
||||
|
||||
pos += totalPacketLen;
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// WebSocket callbacks
|
||||
// ============================================================================
|
||||
|
||||
static void onMessage(WebsocketsMessage message) {
|
||||
std::string raw = message.rawData();
|
||||
size_t len = raw.size();
|
||||
if (len == 0) return;
|
||||
|
||||
const uint8_t* data = reinterpret_cast<const uint8_t*>(raw.data());
|
||||
parseProtocolMessage(data, len);
|
||||
}
|
||||
|
||||
static void onEvent(WebsocketsEvent event, String data) {
|
||||
switch (event) {
|
||||
case WebsocketsEvent::ConnectionOpened:
|
||||
s_connected = true;
|
||||
Serial.println("[WebSocket] Connected");
|
||||
break;
|
||||
case WebsocketsEvent::ConnectionClosed:
|
||||
s_connected = false;
|
||||
Serial.println("[WebSocket] Disconnected");
|
||||
break;
|
||||
case WebsocketsEvent::GotPing:
|
||||
break;
|
||||
case WebsocketsEvent::GotPong:
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Public API
|
||||
// ============================================================================
|
||||
|
||||
void websocketSetup() {
|
||||
WiFi.mode(WIFI_STA);
|
||||
WiFi.begin(WebSocketConfig::WIFI_SSID, WebSocketConfig::WIFI_PASSWORD);
|
||||
Serial.print("[WebSocket] 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("[WebSocket] WiFi failed");
|
||||
return;
|
||||
}
|
||||
Serial.print("[WebSocket] WiFi OK ");
|
||||
Serial.println(WiFi.localIP());
|
||||
|
||||
client.onMessage(onMessage);
|
||||
client.onEvent(onEvent);
|
||||
|
||||
String url = "ws://" + String(WebSocketConfig::HOST) + ":" + String(WebSocketConfig::PORT) + String(WebSocketConfig::PATH);
|
||||
Serial.println("[WebSocket] Connecting to " + url);
|
||||
if (client.connect(url)) {
|
||||
s_connected = true;
|
||||
Serial.println("[WebSocket] Connected");
|
||||
} else {
|
||||
Serial.println("[WebSocket] Connect failed (will retry in loop)");
|
||||
}
|
||||
}
|
||||
|
||||
void websocketLoop() {
|
||||
if (WiFi.status() != WL_CONNECTED) {
|
||||
s_connected = false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!s_connected && !client.available()) {
|
||||
unsigned long now = millis();
|
||||
if (now - lastReconnectAttempt >= RECONNECT_INTERVAL) {
|
||||
lastReconnectAttempt = now;
|
||||
String url = "ws://" + String(WebSocketConfig::HOST) + ":" + String(WebSocketConfig::PORT) + String(WebSocketConfig::PATH);
|
||||
if (client.connect(url)) {
|
||||
s_connected = true;
|
||||
Serial.println("[WebSocket] Reconnected");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (client.available()) {
|
||||
client.poll();
|
||||
}
|
||||
}
|
||||
|
||||
bool websocketConnected() {
|
||||
return s_connected && client.available();
|
||||
}
|
||||
|
|
@ -0,0 +1,22 @@
|
|||
#pragma once
|
||||
#include <Arduino.h>
|
||||
|
||||
// WebSocket client: connect to remote device, receive bytes (e.g. FACE packets).
|
||||
// Extension to the serial protocol - same packet concepts over WebSocket.
|
||||
|
||||
namespace WebSocketConfig {
|
||||
constexpr const char* WIFI_SSID = "Police Surveillance Van";
|
||||
constexpr const char* WIFI_PASSWORD = "ourpassword";
|
||||
constexpr const char* HOST = "192.168.1.206"; // Change to remote device IP
|
||||
constexpr uint16_t PORT = 5001; // Change to remote port
|
||||
constexpr const char* PATH = "/"; // WebSocket path
|
||||
}
|
||||
|
||||
// Call once from setup() after Serial is ready
|
||||
void websocketSetup();
|
||||
|
||||
// Call every loop() - handles connect, reconnect, and incoming messages
|
||||
void websocketLoop();
|
||||
|
||||
// True when connected and ready to send/receive
|
||||
bool websocketConnected();
|
||||
Loading…
Reference in New Issue