added neck motion following eye focus
parent
d7158ea1d0
commit
7e3218afb6
|
|
@ -104,6 +104,8 @@ ushort Crc16Ccitt(byte[] data)
|
|||
|-----|-------------|
|
||||
| `IMU0` | IMU data (accel x,y,z) |
|
||||
| `RDAR` | Radar target data |
|
||||
| `BHVR` | Behavior control (enable/disable) |
|
||||
| `BLST` | Behavior list (list all behaviors and states) |
|
||||
| `STAT` | System state/heartbeat |
|
||||
| `ACK!` | Acknowledge (success) |
|
||||
| `NACK` | Negative acknowledge (failure) |
|
||||
|
|
@ -272,6 +274,33 @@ For each of 3 targets:
|
|||
|
||||
---
|
||||
|
||||
### Behaviors
|
||||
|
||||
#### `BHVR` - Behavior Control (host → device)
|
||||
**Request:**
|
||||
```
|
||||
[behaviorID: 1 byte] // Behavior ID (1 = Focus)
|
||||
[enable: 1 byte] // 0 = disable, non-zero = enable
|
||||
```
|
||||
**Response:** `ACK!` on success, `NACK` on failure
|
||||
|
||||
**Behavior IDs:**
|
||||
- `1` = Focus (radar tracking with motors 14 & 15)
|
||||
|
||||
#### `BLST` - Behavior List (host → device)
|
||||
**Request:** Empty payload
|
||||
|
||||
**Response:**
|
||||
```
|
||||
[count: 1 byte] // Number of registered behaviors
|
||||
[behaviorID1: 1 byte][enabled1: 1 byte] // First behavior
|
||||
[behaviorID2: 1 byte][enabled2: 1 byte] // Second behavior
|
||||
...
|
||||
```
|
||||
- `enabled`: 1 = enabled, 0 = disabled
|
||||
|
||||
---
|
||||
|
||||
### System
|
||||
|
||||
#### `STAT` - System State/Heartbeat (device → host)
|
||||
|
|
|
|||
181
behaviors.cpp
181
behaviors.cpp
|
|
@ -36,21 +36,37 @@ void Behavior::clearMotors() {
|
|||
|
||||
FocusBehavior::FocusBehavior() {
|
||||
isActive = false;
|
||||
currentPosition = POSITION_CENTER;
|
||||
eyePosition = EYE_POSITION_CENTER;
|
||||
neckPosition = NECK_POSITION_CENTER;
|
||||
targetEyePosition = EYE_POSITION_CENTER;
|
||||
targetNeckPosition = NECK_POSITION_CENTER;
|
||||
targetDetectedTime = 0;
|
||||
neckStartTime = 0;
|
||||
neckRotating = false;
|
||||
|
||||
// Add motors 14 and 15 to controlled list
|
||||
// Add motors 14, 15, and 27 to controlled list
|
||||
addMotor(FOCUS_MOTOR_1);
|
||||
addMotor(FOCUS_MOTOR_2);
|
||||
addMotor(NECK_MOTOR);
|
||||
}
|
||||
|
||||
bool FocusBehavior::update() {
|
||||
// Check radar for valid targets
|
||||
uint8_t targetCount = radar.getTargetCount();
|
||||
unsigned long now = millis();
|
||||
|
||||
if (targetCount == 0) {
|
||||
if (targetCount == 0 || !radar.getTarget(0).valid) {
|
||||
// No target - return to center
|
||||
isActive = false;
|
||||
currentPosition = POSITION_CENTER;
|
||||
targetEyePosition = EYE_POSITION_CENTER;
|
||||
targetNeckPosition = NECK_POSITION_CENTER;
|
||||
targetDetectedTime = 0;
|
||||
neckRotating = false;
|
||||
|
||||
// Smoothly interpolate to center
|
||||
eyePosition = lerp(eyePosition, EYE_POSITION_CENTER, INTERPOLATION_SPEED);
|
||||
neckPosition = lerp(neckPosition, NECK_POSITION_CENTER, INTERPOLATION_SPEED);
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -59,63 +75,164 @@ bool FocusBehavior::update() {
|
|||
|
||||
if (!target.valid) {
|
||||
isActive = false;
|
||||
currentPosition = POSITION_CENTER;
|
||||
targetEyePosition = EYE_POSITION_CENTER;
|
||||
targetNeckPosition = NECK_POSITION_CENTER;
|
||||
targetDetectedTime = 0;
|
||||
neckRotating = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Active tracking - calculate position from radar x coordinate
|
||||
// Active tracking - calculate target positions from radar angle
|
||||
isActive = true;
|
||||
currentPosition = calculatePositionFromRadarX(target.x);
|
||||
uint16_t targetEyePos = calculateEyePositionFromRadarAngle(target.angle);
|
||||
uint16_t targetNeckPos = calculateNeckPositionFromRadarAngle(target.angle);
|
||||
|
||||
// Eyes track immediately
|
||||
targetEyePosition = targetEyePos;
|
||||
|
||||
// Check if this is a new target (angle changed significantly or first detection)
|
||||
bool newTarget = (targetDetectedTime == 0) ||
|
||||
(abs((int16_t)targetEyePosition - (int16_t)eyePosition) > 50);
|
||||
|
||||
if (newTarget) {
|
||||
// Reset timing for new target
|
||||
targetDetectedTime = now;
|
||||
neckStartTime = now + NECK_DELAY_MS;
|
||||
neckRotating = false;
|
||||
}
|
||||
|
||||
// Check if neck should start rotating
|
||||
if (!neckRotating && now >= neckStartTime && targetDetectedTime > 0) {
|
||||
neckRotating = true;
|
||||
targetNeckPosition = targetNeckPos;
|
||||
}
|
||||
|
||||
// If neck is rotating, gradually center the eyes as neck approaches target
|
||||
if (neckRotating) {
|
||||
// Neck tracks the target
|
||||
targetNeckPosition = targetNeckPos;
|
||||
|
||||
// Calculate how far the neck has moved toward its target (0 = just started, 1 = at target)
|
||||
int16_t neckDistanceToTarget = abs((int16_t)targetNeckPosition - (int16_t)neckPosition);
|
||||
int16_t totalNeckRange = NECK_POSITION_MAX - NECK_POSITION_MIN;
|
||||
float neckProgress = 1.0f - ((float)neckDistanceToTarget / (float)totalNeckRange);
|
||||
neckProgress = (neckProgress < 0.0f) ? 0.0f : (neckProgress > 1.0f) ? 1.0f : neckProgress;
|
||||
|
||||
// As neck gets closer to target, eyes gradually center
|
||||
// Interpolate eye target between current target position and center based on neck progress
|
||||
float eyeTargetBlend = 1.0f - neckProgress; // 1.0 = at target angle, 0.0 = centered
|
||||
int16_t eyeTargetOffset = (int16_t)targetEyePos - (int16_t)EYE_POSITION_CENTER;
|
||||
targetEyePosition = EYE_POSITION_CENTER + (uint16_t)(eyeTargetOffset * eyeTargetBlend);
|
||||
}
|
||||
|
||||
// Smoothly interpolate current positions toward targets
|
||||
// Use different speeds for eyes and neck (neck is slower/smoother)
|
||||
eyePosition = lerp(eyePosition, targetEyePosition, INTERPOLATION_SPEED);
|
||||
neckPosition = lerp(neckPosition, targetNeckPosition, NECK_INTERPOLATION_SPEED);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool FocusBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
|
||||
// Only provide position for motors we control
|
||||
if (motorID != FOCUS_MOTOR_1 && motorID != FOCUS_MOTOR_2) {
|
||||
return false;
|
||||
// Provide position for eyes (motors 14 and 15)
|
||||
if (motorID == FOCUS_MOTOR_1 || motorID == FOCUS_MOTOR_2) {
|
||||
position = eyePosition;
|
||||
return true;
|
||||
}
|
||||
|
||||
// Always provide position for controlled motors (active by default)
|
||||
// When not tracking a target, currentPosition is set to center in update()
|
||||
position = currentPosition;
|
||||
return true;
|
||||
// Provide position for neck (motor 27)
|
||||
if (motorID == NECK_MOTOR) {
|
||||
position = neckPosition;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
uint16_t FocusBehavior::calculatePositionFromRadarX(float radarX) {
|
||||
// Clamp radar x to valid range
|
||||
if (radarX < RADAR_X_MIN) radarX = RADAR_X_MIN;
|
||||
if (radarX > RADAR_X_MAX) radarX = RADAR_X_MAX;
|
||||
uint16_t FocusBehavior::calculateEyePositionFromRadarAngle(float radarAngle) {
|
||||
// Calculate eye motor position from radar angle (in degrees)
|
||||
// Angle range: approximately -45 to +45 degrees (typical radar FOV)
|
||||
// Map to eye motor position range: 1700 to 2300 (center 2047)
|
||||
|
||||
// Linear mapping from radar x (-80 to 80) to motor position (1700 to 2300)
|
||||
// Center (2047) corresponds to radar x = 0
|
||||
// Left (-80) maps to 1700, Right (80) maps to 2300
|
||||
// Clamp angle to reasonable range (can extend later if needed)
|
||||
constexpr float ANGLE_MIN = -45.0f;
|
||||
constexpr float ANGLE_MAX = 45.0f;
|
||||
|
||||
// Normalize radar x to -1.0 to 1.0 range
|
||||
float normalizedX = radarX / RADAR_X_MAX;
|
||||
if (radarAngle < ANGLE_MIN) radarAngle = ANGLE_MIN;
|
||||
if (radarAngle > ANGLE_MAX) radarAngle = ANGLE_MAX;
|
||||
|
||||
// Normalize angle to -1.0 to 1.0 range
|
||||
float normalizedAngle = radarAngle / ANGLE_MAX;
|
||||
|
||||
// Calculate range from center
|
||||
// Left side: center - 347 = 1700
|
||||
// Right side: center + 253 = 2300
|
||||
float rangeLeft = POSITION_CENTER - POSITION_MIN; // 347
|
||||
float rangeRight = POSITION_MAX - POSITION_CENTER; // 253
|
||||
float rangeLeft = EYE_POSITION_CENTER - EYE_POSITION_MIN; // 347
|
||||
float rangeRight = EYE_POSITION_MAX - EYE_POSITION_CENTER; // 253
|
||||
|
||||
uint16_t position;
|
||||
if (normalizedX < 0.0f) {
|
||||
if (normalizedAngle < 0.0f) {
|
||||
// Left side: use left range
|
||||
position = POSITION_CENTER + (uint16_t)(normalizedX * rangeLeft);
|
||||
position = EYE_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeLeft);
|
||||
} else {
|
||||
// Right side: use right range
|
||||
position = POSITION_CENTER + (uint16_t)(normalizedX * rangeRight);
|
||||
position = EYE_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeRight);
|
||||
}
|
||||
|
||||
// Clamp to valid range
|
||||
if (position < POSITION_MIN) position = POSITION_MIN;
|
||||
if (position > POSITION_MAX) position = POSITION_MAX;
|
||||
if (position < EYE_POSITION_MIN) position = EYE_POSITION_MIN;
|
||||
if (position > EYE_POSITION_MAX) position = EYE_POSITION_MAX;
|
||||
|
||||
return position;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Behavior Manager Implementation
|
||||
// ============================================================================
|
||||
|
|
|
|||
45
behaviors.h
45
behaviors.h
|
|
@ -62,19 +62,44 @@ public:
|
|||
|
||||
private:
|
||||
bool isActive;
|
||||
uint16_t currentPosition; // Current position for motors 14 and 15
|
||||
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;
|
||||
static constexpr uint8_t FOCUS_MOTOR_2 = 15;
|
||||
static constexpr uint16_t POSITION_CENTER = 2047;
|
||||
static constexpr uint16_t POSITION_MIN = 1700;
|
||||
static constexpr uint16_t POSITION_MAX = 2300;
|
||||
static constexpr float RADAR_X_MIN = -80.0f;
|
||||
static constexpr float RADAR_X_MAX = 80.0f;
|
||||
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
|
||||
|
||||
// Calculate motor position from radar x coordinate
|
||||
uint16_t calculatePositionFromRadarX(float radarX);
|
||||
// Eye motor position range (motors 14 and 15)
|
||||
static constexpr uint16_t EYE_POSITION_CENTER = 2047;
|
||||
static constexpr uint16_t EYE_POSITION_MIN = 1600;
|
||||
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);
|
||||
};
|
||||
|
||||
// ============================================================================
|
||||
|
|
|
|||
15
sensors.cpp
15
sensors.cpp
|
|
@ -88,6 +88,14 @@ void Radar::parseFrame() {
|
|||
targets[i].speed = decodeSignMag(spd_raw) * RADAR_DISTANCE_SCALE;
|
||||
|
||||
targets[i].valid = (y_raw != 0) && (y_raw != 0x8000) && (targets[i].y >= RADAR_MIN_VALID_DIST);
|
||||
|
||||
// Calculate angle from x and y (x is mm from center line, y is distance)
|
||||
// atan2(x, y) gives angle in radians, convert to degrees
|
||||
if (targets[i].valid && targets[i].y > 0) {
|
||||
targets[i].angle = atan2(targets[i].x, targets[i].y) * 180.0f / PI;
|
||||
} else {
|
||||
targets[i].angle = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -105,7 +113,7 @@ uint8_t Radar::getTargetCount() const {
|
|||
}
|
||||
|
||||
uint16_t Radar::packPayload(uint8_t* buffer) const {
|
||||
// Format: count(1) + [valid(1), x(2), y(2), speed(2)] * 3 = 22 bytes
|
||||
// Format: count(1) + [valid(1), x(2), y(2), speed(2), angle(2)] * 3 = 28 bytes
|
||||
buffer[0] = getTargetCount();
|
||||
uint16_t offset = 1;
|
||||
|
||||
|
|
@ -115,6 +123,7 @@ uint16_t Radar::packPayload(uint8_t* buffer) const {
|
|||
int16_t x = (int16_t)(targets[i].x * 10); // cm * 10 for precision
|
||||
int16_t y = (int16_t)(targets[i].y * 10);
|
||||
int16_t spd = (int16_t)(targets[i].speed * 10);
|
||||
int16_t angle = (int16_t)(targets[i].angle * 10); // degrees * 10 for precision
|
||||
|
||||
buffer[offset++] = x & 0xFF;
|
||||
buffer[offset++] = (x >> 8) & 0xFF;
|
||||
|
|
@ -122,6 +131,8 @@ uint16_t Radar::packPayload(uint8_t* buffer) const {
|
|||
buffer[offset++] = (y >> 8) & 0xFF;
|
||||
buffer[offset++] = spd & 0xFF;
|
||||
buffer[offset++] = (spd >> 8) & 0xFF;
|
||||
buffer[offset++] = angle & 0xFF;
|
||||
buffer[offset++] = (angle >> 8) & 0xFF;
|
||||
}
|
||||
|
||||
return offset;
|
||||
|
|
@ -322,7 +333,7 @@ void SensorManager::sendADXLPacket() {
|
|||
}
|
||||
|
||||
void SensorManager::sendRadarPacket() {
|
||||
uint8_t payload[32];
|
||||
uint8_t payload[32]; // Updated size for angle field
|
||||
uint16_t len = radar.packPayload(payload);
|
||||
sendPacket(Tag::RADAR, payload, len);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@ struct RadarTarget {
|
|||
float x; // X position in cm (negative=left, positive=right)
|
||||
float y; // Y distance in cm (forward)
|
||||
float speed; // Speed in cm/s
|
||||
float angle; // Angle in degrees (calculated from x,y using atan2)
|
||||
bool valid; // Target is valid
|
||||
};
|
||||
|
||||
|
|
@ -116,7 +117,7 @@ public:
|
|||
private:
|
||||
bool adxlStreamEnabled = true;
|
||||
bool radarStreamEnabled = true;
|
||||
uint16_t adxlInterval = 10;
|
||||
uint16_t adxlInterval = 500; // 500ms = 2 packets per second
|
||||
uint16_t radarInterval = 10;
|
||||
unsigned long lastADXLSend = 0;
|
||||
unsigned long lastRadarSend = 0;
|
||||
|
|
|
|||
Loading…
Reference in New Issue