Compare commits
No commits in common. "main" and "master" have entirely different histories.
62
ANIM_V2
62
ANIM_V2
|
|
@ -1,62 +0,0 @@
|
|||
# Animation File Format Version 2 - Encoding Specification
|
||||
|
||||
## File Structure
|
||||
|
||||
The file consists of three sections in order:
|
||||
|
||||
1. **Filename Block** (optional, for serial transmission)
|
||||
2. **Header Block** (16 bytes)
|
||||
3. **Frame Data Block** (variable size)
|
||||
|
||||
## Encoding Details
|
||||
|
||||
### 1. Filename Block
|
||||
```
|
||||
[filename_length: 2 bytes, uint16_t, little-endian]
|
||||
[filename_bytes: N bytes, UTF-8 string]
|
||||
```
|
||||
|
||||
### 2. Header Block (16 bytes total)
|
||||
```
|
||||
[0-3] "ANIM" (4 bytes, ASCII)
|
||||
[4-5] frameCount (2 bytes, uint16_t, little-endian)
|
||||
[6] version (1 byte, uint8_t) = 2
|
||||
[7] frameRate (1 byte, uint8_t) = FPS
|
||||
[8-15] reserved (8 bytes, all zeros)
|
||||
```
|
||||
|
||||
### 3. Frame Data Block
|
||||
For each frame (0 to frameCount-1), all motors are stored in the same order:
|
||||
|
||||
```
|
||||
For each frame:
|
||||
For each motor (in consistent order):
|
||||
[motor_id: 1 byte, uint8_t]
|
||||
[position: 2 bytes, uint16_t, little-endian, range 0-4095]
|
||||
```
|
||||
|
||||
**Important**:
|
||||
- All frames contain the same motors in the same order
|
||||
- Motor count = (Frame Data Block size) / (frameCount * 3)
|
||||
- Each motor record is exactly 3 bytes: 1 byte ID + 2 bytes position
|
||||
|
||||
## Example File Layout
|
||||
|
||||
For a file with 100 frames and 20 motors:
|
||||
|
||||
```
|
||||
[0-1] Filename length (2 bytes)
|
||||
[2-N] Filename (N bytes)
|
||||
[N+0-N+3] "ANIM" (4 bytes)
|
||||
[N+4-N+5] 100 (frameCount, 2 bytes)
|
||||
[N+6] 2 (version, 1 byte)
|
||||
[N+7] 24 (frameRate, 1 byte)
|
||||
[N+8-N+15] Reserved (8 bytes)
|
||||
[N+16+] Frame data:
|
||||
Frame 0: [motor0_id][motor0_pos][motor1_id][motor1_pos]...[motor19_id][motor19_pos]
|
||||
Frame 1: [motor0_id][motor0_pos][motor1_id][motor1_pos]...[motor19_id][motor19_pos]
|
||||
...
|
||||
Frame 99: [motor0_id][motor0_pos][motor1_id][motor1_pos]...[motor19_id][motor19_pos]
|
||||
```
|
||||
|
||||
Total frame data size = 100 frames × 20 motors × 3 bytes = 6,000 bytes
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -1,545 +0,0 @@
|
|||
# HansonServo Protocol Migration Plan
|
||||
|
||||
## Overview
|
||||
|
||||
The firmware has been updated from a simple XOR-checksum protocol to a more robust CRC16 tagged packet protocol. This document describes the changes needed in the desktop software.
|
||||
|
||||
---
|
||||
|
||||
## Protocol Changes Summary
|
||||
|
||||
| Aspect | Old Protocol | New Protocol |
|
||||
|--------|--------------|--------------|
|
||||
| Sync bytes | `0xAA 0x55` | `0xA5 0x5A` |
|
||||
| Checksum | XOR (1 byte) | CRC16-CCITT (2 bytes) |
|
||||
| Command ID | 1 byte numeric | 4 byte ASCII tag |
|
||||
| Sequence | None | 2 byte counter |
|
||||
| Baud rate | 1,000,000 | 1,000,000 (unchanged) |
|
||||
|
||||
---
|
||||
|
||||
## New Packet Format
|
||||
|
||||
```
|
||||
┌──────┬──────┬─────────┬─────────┬─────────┬───────────┬─────────┐
|
||||
│ SYNC │ SYNC │ TAG │ LENGTH │ SEQ │ PAYLOAD │ CRC16 │
|
||||
│ 0xA5 │ 0x5A │ 4 bytes │ 2 bytes │ 2 bytes │ N bytes │ 2 bytes │
|
||||
└──────┴──────┴─────────┴─────────┴─────────┴───────────┴─────────┘
|
||||
```
|
||||
|
||||
### Field Details
|
||||
|
||||
| Field | Size | Description |
|
||||
|-------|------|-------------|
|
||||
| SYNC0 | 1 | Always `0xA5` |
|
||||
| SYNC1 | 1 | Always `0x5A` |
|
||||
| TAG | 4 | ASCII identifier (e.g., "IDNT", "MSET") |
|
||||
| LENGTH | 2 | Payload length, little-endian |
|
||||
| SEQ | 2 | Sequence number, little-endian |
|
||||
| PAYLOAD | N | Command-specific data |
|
||||
| CRC16 | 2 | CRC16-CCITT over TAG+LENGTH+SEQ+PAYLOAD, little-endian |
|
||||
|
||||
### CRC16-CCITT Implementation
|
||||
|
||||
```python
|
||||
def crc16_ccitt(data: bytes, init: int = 0xFFFF) -> int:
|
||||
crc = init
|
||||
for byte in data:
|
||||
crc ^= byte << 8
|
||||
for _ in range(8):
|
||||
if crc & 0x8000:
|
||||
crc = (crc << 1) ^ 0x1021
|
||||
else:
|
||||
crc <<= 1
|
||||
crc &= 0xFFFF
|
||||
return crc
|
||||
```
|
||||
|
||||
```csharp
|
||||
// C# implementation
|
||||
ushort Crc16Ccitt(byte[] data)
|
||||
{
|
||||
ushort crc = 0xFFFF;
|
||||
foreach (byte b in data)
|
||||
{
|
||||
crc ^= (ushort)(b << 8);
|
||||
for (int i = 0; i < 8; i++)
|
||||
{
|
||||
if ((crc & 0x8000) != 0)
|
||||
crc = (ushort)((crc << 1) ^ 0x1021);
|
||||
else
|
||||
crc <<= 1;
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Command Tag Mapping
|
||||
|
||||
### Old → New Command Mapping
|
||||
|
||||
| Old Command | Old ID | New Tag | Notes |
|
||||
|-------------|--------|---------|-------|
|
||||
| CMD_ID_REQUEST | 0x01 | `IDNT` | Identity request |
|
||||
| CMD_FILE_LIST | 0x02 | `FLST` | List files |
|
||||
| CMD_LOAD_FILE | 0x03 | `FLOD` | Load file content |
|
||||
| CMD_DELETE_FILE | 0x04 | `FDEL` | Delete file |
|
||||
| CMD_SAVE_FILE | 0x05 | `FSAV` | Save animation |
|
||||
| CMD_MESSAGE | 0x06 | `MSGE` | Log/debug message |
|
||||
| CMD_SET_POSITION | 0x07 | `MSET` | Set motor positions |
|
||||
| CMD_PLAY_FILE | 0x08 | `FPLY` | Play animation |
|
||||
| CMD_STOP_FILE | 0x09 | `FSTP` | Stop animation |
|
||||
| CMD_SCAN_CHANNEL | 0x09 | `MSCN` | Scan for motors |
|
||||
| CMD_WRITE_DATA | 0x10 | `MWRT` | Write motor register |
|
||||
| CMD_WRITE_CONFIG_UPDATE | 0x12 | `CONF` | Update config |
|
||||
| CMD_START_POSITION_STREAM | 0x14 | `MSTM` | Motor stream control |
|
||||
| POSITION_STREAM | 0x15 | `MPOS` | Motor position data |
|
||||
|
||||
### New Tags (not in old protocol)
|
||||
|
||||
| Tag | Description |
|
||||
|-----|-------------|
|
||||
| `IMU0` | IMU data (accel x,y,z) |
|
||||
| `RDAR` | Radar target data |
|
||||
| `BHVR` | Behavior control (enable/disable) |
|
||||
| `BLST` | Behavior list (list all behaviors and states) |
|
||||
| `VLST` | List all visemes with names and motor positions |
|
||||
| `VADD` | Add a new viseme with name |
|
||||
| `VDEL` | Delete a viseme by ID |
|
||||
| `VSET` | Set motor positions for a viseme |
|
||||
| `VSME` | Trigger viseme (fire-and-forget) |
|
||||
| `STAT` | System state/heartbeat |
|
||||
| `ACK!` | Acknowledge (success) |
|
||||
| `NACK` | Negative acknowledge (failure) |
|
||||
| `BOOT` | Enter bootloader |
|
||||
|
||||
---
|
||||
|
||||
## Detailed Command Reference
|
||||
|
||||
### Identity & Configuration
|
||||
|
||||
#### `IDNT` - Get Robot Identity
|
||||
**Request:** Empty payload
|
||||
**Response:** Robot config serialized bytes (same format as before)
|
||||
|
||||
#### `CONF` - Update Configuration
|
||||
**Request:** Same payload format as old CMD_WRITE_CONFIG_UPDATE
|
||||
**Response:** `ACK!` on success, `NACK` with reason on failure
|
||||
|
||||
---
|
||||
|
||||
### File Operations
|
||||
|
||||
#### `FLST` - List Files
|
||||
**Request:** Empty payload
|
||||
**Response:** Newline-separated filename list (UTF-8 string)
|
||||
|
||||
#### `FLOD` - Load File
|
||||
**Request:** Filename as raw bytes (no length prefix)
|
||||
**Response:** File contents as raw bytes, or `NACK` if not found
|
||||
|
||||
#### `FSAV` - Save Animation
|
||||
**Request:** Same format as old CMD_SAVE_FILE:
|
||||
```
|
||||
[filename_len: 2 bytes LE]
|
||||
[filename: N bytes]
|
||||
[animation_header: 18 bytes]
|
||||
[curve_segments: variable]
|
||||
[node_graph: variable]
|
||||
```
|
||||
**Response:** `ACK!` on success, `NACK` on failure
|
||||
|
||||
#### `FDEL` - Delete File
|
||||
**Request:**
|
||||
```
|
||||
[filename_len: 2 bytes LE]
|
||||
[filename: N bytes]
|
||||
```
|
||||
**Response:** `ACK!` on success
|
||||
|
||||
#### `FPLY` - Play Animation
|
||||
**Request:**
|
||||
```
|
||||
[filename_len: 2 bytes LE]
|
||||
[filename: N bytes]
|
||||
[play_mode: 1 byte] // 0=idle, 1=once, 2=loop, 3=repeat
|
||||
[repeat_count: 1 byte]
|
||||
[start_frame: 2 bytes LE] // Frame number to start playback from (0-based)
|
||||
```
|
||||
**Response:** `ACK!` on success, `NACK` if file not found
|
||||
|
||||
**Notes:**
|
||||
- `start_frame` allows resuming playback from a specific frame
|
||||
- FRAME packets report actual frame numbers (i.e., if start_frame=163, FRAME packets will show 163, 164, 165...)
|
||||
|
||||
#### `FSTP` - Stop Animation
|
||||
**Request:** Empty payload (0 bytes)
|
||||
**Response:** `ACK!` on success
|
||||
|
||||
**Notes:**
|
||||
- Immediately stops the currently playing animation regardless of play mode
|
||||
- Motors remain in their current positions (torque not disabled)
|
||||
- No FRAME completion packet is sent
|
||||
|
||||
---
|
||||
|
||||
### Motor Control
|
||||
|
||||
#### `MSET` - Set Motor Positions
|
||||
**Request:** Array of motor commands:
|
||||
```
|
||||
[motor_id: 1 byte][position: 2 bytes LE] × N motors
|
||||
```
|
||||
**Response:** `ACK!`
|
||||
|
||||
#### `MPOS` - Motor Position Stream (device → host)
|
||||
**Payload:** Same format as MSET request
|
||||
```
|
||||
[motor_id: 1 byte][position: 2 bytes LE] × N motors
|
||||
```
|
||||
*Sent automatically when streaming is enabled*
|
||||
|
||||
#### `MSCN` - Scan for Motors
|
||||
**Request:**
|
||||
```
|
||||
[channel: 1 byte] // 0 or 1
|
||||
```
|
||||
**Response:** Multiple packets, one per found motor:
|
||||
```
|
||||
[channel: 1][motor_id: 1][model: 2][min_angle: 2][max_angle: 2]
|
||||
[position: 2][cw_dead: 1][ccw_dead: 1][offset: 2][mode: 1]
|
||||
[torque_enable: 1][acceleration: 1][goal_pos: 2][goal_time: 2]
|
||||
[goal_speed: 2][lock: 1][speed: 2][load: 2][temp: 1][moving: 1]
|
||||
[current: 2][voltage: 1]
|
||||
```
|
||||
Final packet has `motor_id = 255` to signal scan complete.
|
||||
|
||||
#### `MWRT` - Write Motor Register
|
||||
**Request:**
|
||||
```
|
||||
[channel: 1 byte]
|
||||
[motor_id: 1 byte]
|
||||
[register: 1 byte]
|
||||
[data_len: 1 byte] // 1 or 2
|
||||
[data: 1-2 bytes]
|
||||
```
|
||||
**Response:** Register read-back value (1 or 2 bytes)
|
||||
|
||||
*Special case:* Register 5 with 1 byte changes the motor ID.
|
||||
|
||||
#### `MSTM` - Motor Stream Control
|
||||
**Request:**
|
||||
```
|
||||
[enable: 1 byte] // 0=disable, 1=enable
|
||||
```
|
||||
**Response:** `ACK!`
|
||||
|
||||
When enabled, device streams `MPOS` packets every 50ms.
|
||||
|
||||
---
|
||||
|
||||
### Sensors
|
||||
|
||||
#### `IMU0` - IMU Data (device → host)
|
||||
**Payload:**
|
||||
```
|
||||
[accelX: 2 bytes LE, signed] // g-forces × 100
|
||||
[accelY: 2 bytes LE, signed] // g-forces × 100
|
||||
[accelZ: 2 bytes LE, signed] // g-forces × 100
|
||||
[pitch: 2 bytes LE, signed] // degrees × 100
|
||||
[roll: 2 bytes LE, signed] // degrees × 100
|
||||
```
|
||||
*Sent automatically when IMU streaming is enabled*
|
||||
|
||||
**Coordinate System:**
|
||||
- X: left/right axis (affects roll)
|
||||
- Y: front/back axis (affects pitch)
|
||||
- Z: up/down axis
|
||||
|
||||
**Notes:**
|
||||
- Acceleration values scaled by 100 (not 1000 as before)
|
||||
- Euler angles calculated from accelerometer data
|
||||
- Heading/yaw not available (accelerometer only)
|
||||
|
||||
#### `RDAR` - Radar Data (device → host)
|
||||
**Payload:**
|
||||
```
|
||||
[target_count: 1 byte]
|
||||
For each of 3 targets:
|
||||
[valid: 1 byte] // 0 or 1
|
||||
[x: 2 bytes LE] // cm × 10, signed
|
||||
[y: 2 bytes LE] // cm × 10, signed
|
||||
[speed: 2 bytes LE] // cm/s × 10, signed
|
||||
```
|
||||
*Sent automatically when radar streaming is enabled*
|
||||
|
||||
---
|
||||
|
||||
### 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 eye motors 14 & 15)
|
||||
- `2` = Idle (perlin noise motion for all motors, ±500 range from center)
|
||||
- `3` = Viseme (mouth motor control for speech)
|
||||
|
||||
#### `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
|
||||
|
||||
---
|
||||
|
||||
### Visemes
|
||||
|
||||
#### `VLST` - List Visemes (host → device)
|
||||
**Request:** Empty payload
|
||||
|
||||
**Response:**
|
||||
```
|
||||
[count: 1 byte] // Number of visemes
|
||||
For each viseme:
|
||||
[viseme_id: 1 byte]
|
||||
[label: 3 bytes] // 3-character label (e.g., "AA ", "SIL")
|
||||
[motor_count: 1 byte]
|
||||
For each motor:
|
||||
[motor_id: 1 byte]
|
||||
[position_low: 1 byte]
|
||||
[position_high: 1 byte]
|
||||
```
|
||||
Position = `position_low | (position_high << 8)`, range 0-4095
|
||||
|
||||
#### `VADD` - Add Viseme (host → device)
|
||||
**Request:**
|
||||
```
|
||||
[label: 3 bytes] // 3-character label (e.g., "AA ", "SIL")
|
||||
```
|
||||
|
||||
**Response:** `ACK!` with payload `[new_viseme_id: 1 byte]` on success, `NACK` on failure
|
||||
|
||||
#### `VDEL` - Delete Viseme (host → device)
|
||||
**Request:**
|
||||
```
|
||||
[viseme_id: 1 byte]
|
||||
```
|
||||
|
||||
**Response:** `ACK!` on success, `NACK` if viseme not found
|
||||
|
||||
#### `VSET` - Set Viseme Motor Positions (host → device)
|
||||
**Request:**
|
||||
```
|
||||
[viseme_id: 1 byte]
|
||||
[motor_count: 1 byte]
|
||||
For each motor:
|
||||
[motor_id: 1 byte]
|
||||
[position_low: 1 byte]
|
||||
[position_high: 1 byte]
|
||||
```
|
||||
|
||||
**Response:** `ACK!` on success, `NACK` if viseme not found
|
||||
|
||||
#### `VSME` - Trigger Viseme (host → device)
|
||||
**Request:**
|
||||
```
|
||||
[viseme_id: 1 byte]
|
||||
```
|
||||
|
||||
**Response:** None (fire-and-forget)
|
||||
|
||||
**Notes:**
|
||||
- Triggers the viseme behavior which controls the motors defined for that viseme
|
||||
- The behavior activates and holds the motor positions
|
||||
- After 3 seconds without a new viseme trigger, the behavior deactivates and releases motor control
|
||||
- Continuously sending viseme IDs will keep the mouth animated
|
||||
- Viseme behavior has higher priority than Idle behavior, lower than Focus
|
||||
|
||||
**Default Viseme IDs (loaded at startup):**
|
||||
- `0` = Neutral/rest (sil) - motors 40, 43, 44
|
||||
- `1` = AA (as in "father")
|
||||
- `2` = AE (as in "cat")
|
||||
- `3` = AH (as in "but")
|
||||
- `4` = AO (as in "bought")
|
||||
- `5` = EH (as in "bed")
|
||||
- `6` = IH (as in "bit")
|
||||
- `7` = IY (as in "beat")
|
||||
- `8` = OW (as in "boat")
|
||||
- `9` = UH (as in "book")
|
||||
- `10` = UW (as in "boot")
|
||||
|
||||
---
|
||||
|
||||
### System
|
||||
|
||||
#### `STAT` - System State/Heartbeat (device → host)
|
||||
**Payload:**
|
||||
```
|
||||
[uptime: 4 bytes LE] // seconds since boot
|
||||
[flags: 2 bytes LE] // bit flags
|
||||
```
|
||||
**Flags:**
|
||||
- Bit 0: IMU ready
|
||||
- Bit 1: Animation playing
|
||||
- Bit 2: Motor streaming active
|
||||
- Bit 3: IMU streaming active
|
||||
- Bit 4: Radar streaming active
|
||||
|
||||
*Sent automatically every 1 second*
|
||||
|
||||
#### `MSGE` - Log Message (device → host)
|
||||
**Payload:** UTF-8 string (no null terminator)
|
||||
|
||||
#### `ACK!` - Acknowledge
|
||||
**Payload:**
|
||||
```
|
||||
[original_tag: 4 bytes] // The tag being acknowledged
|
||||
```
|
||||
|
||||
#### `NACK` - Negative Acknowledge
|
||||
**Payload:**
|
||||
```
|
||||
[original_tag: 4 bytes]
|
||||
[reason: N bytes, optional UTF-8 string]
|
||||
```
|
||||
|
||||
#### `BOOT` - Enter Bootloader
|
||||
**Request:** Empty payload
|
||||
**Response:** `MSGE` "Entering bootloader...", then device resets
|
||||
|
||||
---
|
||||
|
||||
## Implementation Checklist
|
||||
|
||||
### 1. Protocol Layer Changes
|
||||
|
||||
- [ ] Update sync byte detection from `0xAA 0x55` to `0xA5 0x5A`
|
||||
- [ ] Implement CRC16-CCITT calculation
|
||||
- [ ] Update packet parsing to handle new format:
|
||||
- [ ] Read 4-byte tag instead of 1-byte command
|
||||
- [ ] Read 2-byte sequence number (can ignore for now, or use for debugging)
|
||||
- [ ] Verify CRC16 instead of XOR checksum
|
||||
- [ ] Update packet building:
|
||||
- [ ] Use 4-byte tags
|
||||
- [ ] Add sequence counter (increment per packet)
|
||||
- [ ] Calculate and append CRC16
|
||||
|
||||
### 2. Command Handler Updates
|
||||
|
||||
- [ ] Replace command ID constants with tag strings
|
||||
- [ ] Update request builders for each command
|
||||
- [ ] Update response parsers for each command
|
||||
- [ ] Add handlers for new response types:
|
||||
- [ ] `ACK!` - generic success
|
||||
- [ ] `NACK` - generic failure with reason
|
||||
- [ ] `STAT` - heartbeat (can use to detect connection)
|
||||
- [ ] `IMU0` - IMU data (if needed)
|
||||
- [ ] `RDAR` - radar data (if needed)
|
||||
|
||||
### 3. UI/UX Improvements (Optional)
|
||||
|
||||
- [ ] Show connection status based on `STAT` heartbeat
|
||||
- [ ] Display IMU orientation if sensor is available
|
||||
- [ ] Show radar targets if sensor is available
|
||||
|
||||
---
|
||||
|
||||
## Example: Sending a Motor Position Command
|
||||
|
||||
### Old Code (pseudocode)
|
||||
```python
|
||||
def send_motor_positions(motors):
|
||||
payload = b''
|
||||
for motor_id, position in motors:
|
||||
payload += bytes([motor_id])
|
||||
payload += struct.pack('<H', position)
|
||||
|
||||
length = len(payload)
|
||||
checksum = CMD_SET_POSITION ^ (length >> 8) ^ (length & 0xFF)
|
||||
for b in payload:
|
||||
checksum ^= b
|
||||
|
||||
packet = bytes([0xAA, 0x55, CMD_SET_POSITION])
|
||||
packet += struct.pack('>H', length) # big-endian length
|
||||
packet += payload
|
||||
packet += bytes([checksum])
|
||||
|
||||
serial.write(packet)
|
||||
```
|
||||
|
||||
### New Code (pseudocode)
|
||||
```python
|
||||
def send_motor_positions(motors):
|
||||
tag = b'MSET'
|
||||
payload = b''
|
||||
for motor_id, position in motors:
|
||||
payload += bytes([motor_id])
|
||||
payload += struct.pack('<H', position)
|
||||
|
||||
length = len(payload)
|
||||
seq = get_next_sequence()
|
||||
|
||||
# Build data for CRC: tag + length + seq + payload
|
||||
crc_data = tag
|
||||
crc_data += struct.pack('<H', length)
|
||||
crc_data += struct.pack('<H', seq)
|
||||
crc_data += payload
|
||||
crc = crc16_ccitt(crc_data)
|
||||
|
||||
packet = bytes([0xA5, 0x5A])
|
||||
packet += tag
|
||||
packet += struct.pack('<H', length)
|
||||
packet += struct.pack('<H', seq)
|
||||
packet += payload
|
||||
packet += struct.pack('<H', crc)
|
||||
|
||||
serial.write(packet)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Testing Strategy
|
||||
|
||||
1. **Connection Test:** Send empty `IDNT` request, expect `IDNT` response with config
|
||||
2. **File List Test:** Send `FLST`, expect filename list
|
||||
3. **Motor Test:** Send `MSET` with known positions, expect `ACK!`
|
||||
4. **Heartbeat:** After connecting, should receive `STAT` packets every second
|
||||
|
||||
---
|
||||
|
||||
## Backwards Compatibility
|
||||
|
||||
The new protocol uses different sync bytes (`0xA5 0x5A` vs `0xAA 0x55`), so there's no ambiguity. If you need to support both old and new firmware:
|
||||
|
||||
1. Detect firmware version by sync bytes in received packets
|
||||
2. Switch protocol handler based on detected version
|
||||
3. Or: just update all firmware to new version
|
||||
|
||||
---
|
||||
|
||||
## Questions?
|
||||
|
||||
The firmware source is at:
|
||||
`C:\Users\jake\Documents\hansonProjects\HansonServo\`
|
||||
|
||||
Key files:
|
||||
- `protocol.h/cpp` - Packet format and CRC implementation
|
||||
- `commands.h/cpp` - Command handlers
|
||||
- `sensors.h/cpp` - IMU and radar drivers
|
||||
|
||||
202
animation.cpp
202
animation.cpp
|
|
@ -81,25 +81,6 @@ uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) {
|
|||
|
||||
void Animation::clear() {
|
||||
//memset(data, 0, sizeof(data));
|
||||
frameData.clear();
|
||||
}
|
||||
|
||||
void Animation::setFrameData(uint16_t frameIndex, const std::vector<MotorPosition>& motors) {
|
||||
if (frameIndex >= frameData.size()) {
|
||||
frameData.resize(frameIndex + 1);
|
||||
}
|
||||
frameData[frameIndex] = motors;
|
||||
}
|
||||
|
||||
const std::vector<MotorPosition>* Animation::getFrameData(uint16_t frameIndex) const {
|
||||
if (frameIndex >= frameData.size()) {
|
||||
return nullptr;
|
||||
}
|
||||
return &frameData[frameIndex];
|
||||
}
|
||||
|
||||
void Animation::clearFrameData() {
|
||||
frameData.clear();
|
||||
}
|
||||
|
||||
// uint16_t* Animation::getRawData() {
|
||||
|
|
@ -124,32 +105,20 @@ bool Animation::saveToFile(const char* filename) {
|
|||
|
||||
file.write((uint8_t*)&header, sizeof(header));
|
||||
|
||||
if (header.version == 2) {
|
||||
// Version 2: Write frame data
|
||||
// For each frame, write all motor positions
|
||||
for (uint16_t frameIndex = 0; frameIndex < frameData.size() && frameIndex < header.frameCount; frameIndex++) {
|
||||
const auto& frame = frameData[frameIndex];
|
||||
for (const auto& motorPos : frame) {
|
||||
file.write((uint8_t*)&motorPos, sizeof(MotorPosition));
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// Version 1: Write curves and node graph
|
||||
uint16_t curveCount = 0;
|
||||
for (const auto& [motorID, segments] : curves) {
|
||||
curveCount += segments.size();
|
||||
}
|
||||
file.write((uint8_t*)&curveCount, sizeof(curveCount));
|
||||
for (const auto& [motorID, segments] : curves) {
|
||||
for (const CurveSegment& seg : segments) {
|
||||
file.write((uint8_t*)&seg, sizeof(CurveSegment));
|
||||
}
|
||||
}
|
||||
|
||||
// ✅ Write serialized node graph
|
||||
std::vector<uint8_t> graphData = nodeGraph.serialize();
|
||||
file.write(graphData.data(), graphData.size());
|
||||
uint16_t curveCount = 0;
|
||||
for (const auto& [motorID, segments] : curves) {
|
||||
curveCount += segments.size();
|
||||
}
|
||||
file.write((uint8_t*)&curveCount, sizeof(curveCount));
|
||||
for (const auto& [motorID, segments] : curves) {
|
||||
for (const CurveSegment& seg : segments) {
|
||||
file.write((uint8_t*)&seg, sizeof(CurveSegment));
|
||||
}
|
||||
}
|
||||
|
||||
// ✅ Write serialized node graph
|
||||
std::vector<uint8_t> graphData = nodeGraph.serialize();
|
||||
file.write(graphData.data(), graphData.size());
|
||||
|
||||
file.close();
|
||||
return true;
|
||||
|
|
@ -167,87 +136,46 @@ bool Animation::loadFromFile(const char* filename) {
|
|||
return false;
|
||||
}
|
||||
|
||||
if (strncmp(tempHeader.magic, "ANIM", 4) != 0) {
|
||||
file.close();
|
||||
return false;
|
||||
}
|
||||
|
||||
if (tempHeader.version != 1 && tempHeader.version != 2) {
|
||||
if (strncmp(tempHeader.magic, "ANIM", 4) != 0 || tempHeader.version != 1) {
|
||||
file.close();
|
||||
return false;
|
||||
}
|
||||
|
||||
header = tempHeader;
|
||||
|
||||
if (header.version == 2) {
|
||||
// Version 2: Read frame data
|
||||
clearFrameData();
|
||||
frameData.reserve(header.frameCount);
|
||||
|
||||
// Calculate motor count from file size
|
||||
size_t fileSize = file.size();
|
||||
size_t headerSize = sizeof(AnimationHeader);
|
||||
size_t frameDataSize = fileSize - headerSize;
|
||||
size_t frameSize = frameDataSize / header.frameCount; // bytes per frame
|
||||
uint16_t motorCount = frameSize / sizeof(MotorPosition); // motors per frame
|
||||
|
||||
if (frameSize % sizeof(MotorPosition) != 0) {
|
||||
// Read curve count
|
||||
uint16_t curveCount;
|
||||
if (file.read((uint8_t*)&curveCount, sizeof(curveCount)) != sizeof(curveCount)) {
|
||||
file.close();
|
||||
return false;
|
||||
}
|
||||
|
||||
clearAllCurves();
|
||||
|
||||
// Read curve segments
|
||||
for (uint16_t i = 0; i < curveCount; i++) {
|
||||
CurveSegment seg;
|
||||
if (file.read((uint8_t*)&seg, sizeof(CurveSegment)) != sizeof(CurveSegment)) {
|
||||
file.close();
|
||||
return false; // Invalid frame data size
|
||||
return false;
|
||||
}
|
||||
|
||||
// Read all frames
|
||||
for (uint16_t frameIndex = 0; frameIndex < header.frameCount; frameIndex++) {
|
||||
std::vector<MotorPosition> frame;
|
||||
frame.reserve(motorCount);
|
||||
|
||||
for (uint16_t motorIndex = 0; motorIndex < motorCount; motorIndex++) {
|
||||
MotorPosition motorPos;
|
||||
if (file.read((uint8_t*)&motorPos, sizeof(MotorPosition)) != sizeof(MotorPosition)) {
|
||||
file.close();
|
||||
return false;
|
||||
}
|
||||
frame.push_back(motorPos);
|
||||
}
|
||||
|
||||
frameData.push_back(frame);
|
||||
}
|
||||
} else {
|
||||
// Version 1: Read curves and node graph
|
||||
// Read curve count
|
||||
uint16_t curveCount;
|
||||
if (file.read((uint8_t*)&curveCount, sizeof(curveCount)) != sizeof(curveCount)) {
|
||||
curves[seg.motorID].push_back(seg);
|
||||
}
|
||||
|
||||
// ✅ Read remaining bytes into buffer
|
||||
size_t remaining = file.available();
|
||||
if (remaining > 0) {
|
||||
std::vector<uint8_t> buffer(remaining);
|
||||
if (file.read(buffer.data(), remaining) != remaining) {
|
||||
file.close();
|
||||
return false;
|
||||
}
|
||||
|
||||
clearAllCurves();
|
||||
|
||||
// Read curve segments
|
||||
for (uint16_t i = 0; i < curveCount; i++) {
|
||||
CurveSegment seg;
|
||||
if (file.read((uint8_t*)&seg, sizeof(CurveSegment)) != sizeof(CurveSegment)) {
|
||||
file.close();
|
||||
return false;
|
||||
}
|
||||
curves[seg.motorID].push_back(seg);
|
||||
}
|
||||
|
||||
// ✅ Read remaining bytes into buffer
|
||||
size_t remaining = file.available();
|
||||
if (remaining > 0) {
|
||||
std::vector<uint8_t> buffer(remaining);
|
||||
if (file.read(buffer.data(), remaining) != remaining) {
|
||||
file.close();
|
||||
return false;
|
||||
}
|
||||
|
||||
// ✅ Load node graph from buffer
|
||||
nodeGraph.nodes.clear();
|
||||
nodeGraph.connections.clear();
|
||||
loadNodeGraph(buffer.data(), buffer.size(), nodeGraph);
|
||||
nodeGraph.bindAnimationContext(this);
|
||||
}
|
||||
// ✅ Load node graph from buffer
|
||||
nodeGraph.nodes.clear();
|
||||
nodeGraph.connections.clear();
|
||||
loadNodeGraph(buffer.data(), buffer.size(), nodeGraph);
|
||||
nodeGraph.bindAnimationContext(this);
|
||||
}
|
||||
|
||||
file.close();
|
||||
|
|
@ -283,54 +211,6 @@ String Animation::printCurves() {
|
|||
return output;
|
||||
}
|
||||
|
||||
String Animation::printAnim() {
|
||||
String output = "ANIMATION INFO\n";
|
||||
output += "==============\n";
|
||||
output += "Version: " + String(header.version) + "\n";
|
||||
output += "Frame Count: " + String(header.frameCount) + "\n";
|
||||
output += "Frame Rate: " + String(header.frameRate) + " fps\n";
|
||||
|
||||
if (header.frameRate > 0) {
|
||||
float duration = (float)header.frameCount / (float)header.frameRate;
|
||||
output += "Duration: " + String(duration, 2) + " seconds\n";
|
||||
}
|
||||
|
||||
output += "Active: " + String(isActive() ? "Yes" : "No") + "\n";
|
||||
|
||||
if (header.version == 1) {
|
||||
// Version 1: curves and node graph
|
||||
uint16_t curveCount = 0;
|
||||
for (const auto& [motorID, segments] : curves) {
|
||||
curveCount += segments.size();
|
||||
}
|
||||
output += "Curve Segments: " + String(curveCount) + "\n";
|
||||
output += "Motors with Curves: " + String(curves.size()) + "\n";
|
||||
output += "Node Graph Nodes: " + String(nodeGraph.nodes.size()) + "\n";
|
||||
output += "Node Graph Connections: " + String(nodeGraph.connections.size()) + "\n";
|
||||
} else if (header.version == 2) {
|
||||
// Version 2: frame data
|
||||
if (!frameData.empty()) {
|
||||
uint16_t motorCount = frameData[0].size();
|
||||
output += "Motors per Frame: " + String(motorCount) + "\n";
|
||||
output += "Frames Stored: " + String(frameData.size()) + "\n";
|
||||
|
||||
// Show motor IDs from first frame
|
||||
if (!frameData[0].empty()) {
|
||||
output += "Motor IDs: ";
|
||||
for (size_t i = 0; i < frameData[0].size(); i++) {
|
||||
if (i > 0) output += ", ";
|
||||
output += String(frameData[0][i].motorID);
|
||||
}
|
||||
output += "\n";
|
||||
}
|
||||
} else {
|
||||
output += "Frame Data: Empty\n";
|
||||
}
|
||||
}
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
16
animation.h
16
animation.h
|
|
@ -4,7 +4,6 @@
|
|||
#include "FS.h"
|
||||
#include "FFat.h"
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
#include "nodegraph.h"
|
||||
|
||||
|
||||
|
|
@ -35,12 +34,6 @@ struct __attribute__((packed)) CurveSegment {
|
|||
int16_t endPointY;
|
||||
};
|
||||
|
||||
// Version 2 frame data: motor ID + position pair
|
||||
struct __attribute__((packed)) MotorPosition {
|
||||
uint8_t motorID;
|
||||
uint16_t position; // 0-4095
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -58,14 +51,8 @@ public:
|
|||
void clearCurves(uint8_t motorID);
|
||||
void clearAllCurves();
|
||||
String printCurves();
|
||||
String printAnim();
|
||||
uint16_t getMotorPosition(uint8_t motorID, uint16_t timeCS);
|
||||
|
||||
// Version 2 frame data methods
|
||||
void setFrameData(uint16_t frameIndex, const std::vector<MotorPosition>& motors);
|
||||
const std::vector<MotorPosition>* getFrameData(uint16_t frameIndex) const;
|
||||
void clearFrameData();
|
||||
|
||||
void clear();
|
||||
//uint16_t* getRawData(); // Optional: for bulk access
|
||||
//size_t getSize() const;
|
||||
|
|
@ -81,8 +68,7 @@ public:
|
|||
|
||||
private:
|
||||
//uint16_t data[MAX_FRAMES][NUM_CHANNELS];
|
||||
std::unordered_map<uint8_t, std::vector<CurveSegment>> curves; // Version 1: curves
|
||||
std::vector<std::vector<MotorPosition>> frameData; // Version 2: raw frame data
|
||||
std::unordered_map<uint8_t, std::vector<CurveSegment>> curves;
|
||||
bool active = false;
|
||||
};
|
||||
|
||||
|
|
|
|||
700
behaviors.cpp
700
behaviors.cpp
|
|
@ -1,700 +0,0 @@
|
|||
#include "behaviors.h"
|
||||
#include <algorithm>
|
||||
|
||||
// ============================================================================
|
||||
// Base Behavior Implementation
|
||||
// ============================================================================
|
||||
|
||||
Behavior::Behavior() {
|
||||
controlledMotors.clear();
|
||||
}
|
||||
|
||||
void Behavior::addMotor(uint8_t motorID) {
|
||||
// Check if motor already in list
|
||||
for (uint8_t id : controlledMotors) {
|
||||
if (id == motorID) {
|
||||
return; // Already added
|
||||
}
|
||||
}
|
||||
controlledMotors.push_back(motorID);
|
||||
}
|
||||
|
||||
void Behavior::removeMotor(uint8_t motorID) {
|
||||
controlledMotors.erase(
|
||||
std::remove(controlledMotors.begin(), controlledMotors.end(), motorID),
|
||||
controlledMotors.end()
|
||||
);
|
||||
}
|
||||
|
||||
void Behavior::clearMotors() {
|
||||
controlledMotors.clear();
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Focus Behavior Implementation
|
||||
// ============================================================================
|
||||
|
||||
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;
|
||||
|
||||
// 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 || !radar.getTarget(0).valid) {
|
||||
// No target - return to center
|
||||
isActive = false;
|
||||
targetEyePosition = EYE_POSITION_CENTER;
|
||||
targetNeckPosition = NECK_POSITION_CENTER;
|
||||
targetDetectedTime = 0;
|
||||
neckRotating = false;
|
||||
|
||||
// Smoothly interpolate eyes to center
|
||||
eyePosition = lerp(eyePosition, EYE_POSITION_CENTER, INTERPOLATION_SPEED);
|
||||
|
||||
// Keep neck at center (no movement)
|
||||
neckPosition = NECK_POSITION_CENTER;
|
||||
|
||||
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
|
||||
isActive = true;
|
||||
uint16_t targetEyePos = calculateEyePositionFromRadarAngle(target.angle);
|
||||
|
||||
// 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;
|
||||
|
||||
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) {
|
||||
position = eyePosition;
|
||||
return true;
|
||||
}
|
||||
|
||||
// Provide position for neck (motor 27)
|
||||
if (motorID == NECK_MOTOR) {
|
||||
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);
|
||||
} else {
|
||||
// Positive angle: use right range (300 units)
|
||||
positionFloat = (float)EYE_POSITION_CENTER + (normalized * rangeRight);
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Idle Behavior Implementation
|
||||
// ============================================================================
|
||||
|
||||
IdleBehavior::IdleBehavior() {
|
||||
startTime = millis();
|
||||
|
||||
// Initialize all motor positions to center
|
||||
for (int i = 0; i < 256; i++) {
|
||||
motorPositions[i] = POSITION_CENTER;
|
||||
}
|
||||
}
|
||||
|
||||
void IdleBehavior::initMotors(const std::vector<uint8_t>& motorIDs) {
|
||||
clearMotors();
|
||||
for (uint8_t id : motorIDs) {
|
||||
addMotor(id);
|
||||
motorPositions[id] = POSITION_CENTER;
|
||||
}
|
||||
}
|
||||
|
||||
bool IdleBehavior::update() {
|
||||
unsigned long now = millis();
|
||||
float timeOffset = (float)(now - startTime) * NOISE_SPEED;
|
||||
|
||||
// Update position for each controlled motor using perlin noise
|
||||
for (uint8_t motorID : controlledMotors) {
|
||||
// Use motor ID as seed offset for variety between motors
|
||||
uint16_t seed = motorID * MOTOR_SEED_OFFSET;
|
||||
|
||||
// Get perlin noise value (-1 to 1 range approximately)
|
||||
float noiseValue = perlin1D_octave(seed, timeOffset, 3, 0.5f);
|
||||
|
||||
// Map noise to position range: center ± NOISE_RANGE
|
||||
// Perlin noise typically returns values in roughly -1 to 1 range
|
||||
int16_t offset = (int16_t)(noiseValue * (float)NOISE_RANGE);
|
||||
|
||||
// Calculate final position
|
||||
int16_t position = (int16_t)POSITION_CENTER + offset;
|
||||
|
||||
// Clamp to valid servo range
|
||||
if (position < 1547) position = 1547; // center - 500
|
||||
if (position > 2547) position = 2547; // center + 500
|
||||
|
||||
motorPositions[motorID] = (uint16_t)position;
|
||||
}
|
||||
|
||||
// Idle behavior is always active (but low priority)
|
||||
return true;
|
||||
}
|
||||
|
||||
bool IdleBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
|
||||
// Check if we control this motor
|
||||
for (uint8_t id : controlledMotors) {
|
||||
if (id == motorID) {
|
||||
position = motorPositions[motorID];
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Viseme Behavior Implementation
|
||||
// ============================================================================
|
||||
|
||||
VisemeBehavior::VisemeBehavior() {
|
||||
isActive = false;
|
||||
lastTriggerTime = 0;
|
||||
nextVisemeID = 0;
|
||||
currentPositions.clear();
|
||||
visemes.clear();
|
||||
}
|
||||
|
||||
Viseme* VisemeBehavior::findViseme(uint8_t id) {
|
||||
for (Viseme& v : visemes) {
|
||||
if (v.id == id) {
|
||||
return &v;
|
||||
}
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
uint8_t VisemeBehavior::addViseme(const char* label) {
|
||||
Viseme newViseme;
|
||||
newViseme.id = nextVisemeID++;
|
||||
|
||||
// Copy label (3 chars, ensure null-terminated)
|
||||
if (label && strlen(label) >= 3) {
|
||||
newViseme.label[0] = label[0];
|
||||
newViseme.label[1] = label[1];
|
||||
newViseme.label[2] = label[2];
|
||||
newViseme.label[3] = '\0';
|
||||
} else {
|
||||
// Default label if not provided or too short
|
||||
newViseme.label[0] = 'V';
|
||||
newViseme.label[1] = 'I';
|
||||
newViseme.label[2] = 'S';
|
||||
newViseme.label[3] = '\0';
|
||||
}
|
||||
|
||||
newViseme.motorPositions.clear();
|
||||
visemes.push_back(newViseme);
|
||||
|
||||
Serial.print("[Viseme] Added viseme '");
|
||||
Serial.print(newViseme.label);
|
||||
Serial.print("' with ID ");
|
||||
Serial.println(newViseme.id);
|
||||
|
||||
return newViseme.id;
|
||||
}
|
||||
|
||||
void VisemeBehavior::addViseme(uint8_t id, uint16_t pos40, uint16_t pos43, uint16_t pos44) {
|
||||
// Legacy method for backwards compatibility
|
||||
Viseme* existing = findViseme(id);
|
||||
|
||||
if (existing) {
|
||||
// Update existing viseme
|
||||
existing->motorPositions.clear();
|
||||
existing->motorPositions.push_back({40, pos40});
|
||||
existing->motorPositions.push_back({43, pos43});
|
||||
existing->motorPositions.push_back({44, pos44});
|
||||
} else {
|
||||
// Add new viseme
|
||||
Viseme newViseme;
|
||||
newViseme.id = id;
|
||||
|
||||
// Default label based on ID (V + 2 digit ID)
|
||||
newViseme.label[0] = 'V';
|
||||
if (id < 10) {
|
||||
newViseme.label[1] = '0' + id;
|
||||
newViseme.label[2] = ' ';
|
||||
} else if (id < 100) {
|
||||
newViseme.label[1] = '0' + (id / 10);
|
||||
newViseme.label[2] = '0' + (id % 10);
|
||||
} else {
|
||||
newViseme.label[1] = 'X';
|
||||
newViseme.label[2] = 'X';
|
||||
}
|
||||
newViseme.label[3] = '\0';
|
||||
|
||||
newViseme.motorPositions.push_back({40, pos40});
|
||||
newViseme.motorPositions.push_back({43, pos43});
|
||||
newViseme.motorPositions.push_back({44, pos44});
|
||||
visemes.push_back(newViseme);
|
||||
|
||||
// Update nextVisemeID if needed
|
||||
if (id >= nextVisemeID) {
|
||||
nextVisemeID = id + 1;
|
||||
}
|
||||
}
|
||||
|
||||
// Update controlled motors list
|
||||
addMotor(40);
|
||||
addMotor(43);
|
||||
addMotor(44);
|
||||
}
|
||||
|
||||
// Overload to add viseme with explicit label
|
||||
void VisemeBehavior::addViseme(uint8_t id, const char* label, uint16_t pos40, uint16_t pos43, uint16_t pos44) {
|
||||
Viseme* existing = findViseme(id);
|
||||
|
||||
if (existing) {
|
||||
// Update existing viseme
|
||||
if (label) {
|
||||
existing->label[0] = label[0];
|
||||
existing->label[1] = label[1];
|
||||
existing->label[2] = label[2];
|
||||
existing->label[3] = '\0';
|
||||
}
|
||||
existing->motorPositions.clear();
|
||||
existing->motorPositions.push_back({40, pos40});
|
||||
existing->motorPositions.push_back({43, pos43});
|
||||
existing->motorPositions.push_back({44, pos44});
|
||||
} else {
|
||||
// Add new viseme
|
||||
Viseme newViseme;
|
||||
newViseme.id = id;
|
||||
|
||||
// Set label
|
||||
if (label) {
|
||||
newViseme.label[0] = label[0];
|
||||
newViseme.label[1] = label[1];
|
||||
newViseme.label[2] = label[2];
|
||||
newViseme.label[3] = '\0';
|
||||
} else {
|
||||
// Default label
|
||||
newViseme.label[0] = 'V';
|
||||
if (id < 10) {
|
||||
newViseme.label[1] = '0' + id;
|
||||
newViseme.label[2] = ' ';
|
||||
} else if (id < 100) {
|
||||
newViseme.label[1] = '0' + (id / 10);
|
||||
newViseme.label[2] = '0' + (id % 10);
|
||||
} else {
|
||||
newViseme.label[1] = 'X';
|
||||
newViseme.label[2] = 'X';
|
||||
}
|
||||
newViseme.label[3] = '\0';
|
||||
}
|
||||
|
||||
newViseme.motorPositions.push_back({40, pos40});
|
||||
newViseme.motorPositions.push_back({43, pos43});
|
||||
newViseme.motorPositions.push_back({44, pos44});
|
||||
visemes.push_back(newViseme);
|
||||
|
||||
// Update nextVisemeID if needed
|
||||
if (id >= nextVisemeID) {
|
||||
nextVisemeID = id + 1;
|
||||
}
|
||||
}
|
||||
|
||||
// Update controlled motors list
|
||||
addMotor(40);
|
||||
addMotor(43);
|
||||
addMotor(44);
|
||||
}
|
||||
|
||||
bool VisemeBehavior::deleteViseme(uint8_t visemeID) {
|
||||
for (auto it = visemes.begin(); it != visemes.end(); ++it) {
|
||||
if (it->id == visemeID) {
|
||||
Serial.print("[Viseme] Deleted viseme ID ");
|
||||
Serial.println(visemeID);
|
||||
visemes.erase(it);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
Serial.print("[Viseme] Delete failed - unknown viseme ID ");
|
||||
Serial.println(visemeID);
|
||||
return false;
|
||||
}
|
||||
|
||||
bool VisemeBehavior::setVisemeMotors(uint8_t visemeID, const std::vector<VisemeMotorPosition>& positions) {
|
||||
Viseme* viseme = findViseme(visemeID);
|
||||
if (!viseme) {
|
||||
Serial.print("[Viseme] setVisemeMotors failed - unknown viseme ID ");
|
||||
Serial.println(visemeID);
|
||||
return false;
|
||||
}
|
||||
|
||||
// Update motor positions
|
||||
viseme->motorPositions = positions;
|
||||
|
||||
// Update controlled motors list
|
||||
for (const auto& pos : positions) {
|
||||
addMotor(pos.motorID);
|
||||
}
|
||||
|
||||
Serial.print("[Viseme] Updated viseme ID ");
|
||||
Serial.print(visemeID);
|
||||
Serial.print(" with ");
|
||||
Serial.print(positions.size());
|
||||
Serial.println(" motors");
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool VisemeBehavior::setVisemeMotorsAndLabel(uint8_t visemeID, const char* label, const std::vector<VisemeMotorPosition>& positions) {
|
||||
Viseme* viseme = findViseme(visemeID);
|
||||
if (!viseme) {
|
||||
Serial.print("[Viseme] setVisemeMotorsAndLabel failed - unknown viseme ID ");
|
||||
Serial.println(visemeID);
|
||||
return false;
|
||||
}
|
||||
|
||||
// Update label (3 bytes)
|
||||
if (label) {
|
||||
viseme->label[0] = label[0];
|
||||
viseme->label[1] = label[1];
|
||||
viseme->label[2] = label[2];
|
||||
viseme->label[3] = '\0';
|
||||
}
|
||||
|
||||
// Update motor positions
|
||||
viseme->motorPositions = positions;
|
||||
|
||||
// Update controlled motors list
|
||||
for (const auto& pos : positions) {
|
||||
addMotor(pos.motorID);
|
||||
}
|
||||
|
||||
Serial.print("[Viseme] Updated viseme ID ");
|
||||
Serial.print(visemeID);
|
||||
Serial.print(" label '");
|
||||
Serial.print(viseme->label);
|
||||
Serial.print("' with ");
|
||||
Serial.print(positions.size());
|
||||
Serial.println(" motors");
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool VisemeBehavior::createOrUpdateViseme(uint8_t visemeID, const char* label, const std::vector<VisemeMotorPosition>& positions) {
|
||||
Viseme* viseme = findViseme(visemeID);
|
||||
|
||||
if (viseme) {
|
||||
// Update existing
|
||||
return setVisemeMotorsAndLabel(visemeID, label, positions);
|
||||
} else {
|
||||
// Create new
|
||||
Viseme newViseme;
|
||||
newViseme.id = visemeID;
|
||||
|
||||
// Set label
|
||||
if (label) {
|
||||
newViseme.label[0] = label[0];
|
||||
newViseme.label[1] = label[1];
|
||||
newViseme.label[2] = label[2];
|
||||
newViseme.label[3] = '\0';
|
||||
} else {
|
||||
// Default label
|
||||
newViseme.label[0] = 'V';
|
||||
if (visemeID < 10) {
|
||||
newViseme.label[1] = '0' + visemeID;
|
||||
newViseme.label[2] = ' ';
|
||||
} else if (visemeID < 100) {
|
||||
newViseme.label[1] = '0' + (visemeID / 10);
|
||||
newViseme.label[2] = '0' + (visemeID % 10);
|
||||
} else {
|
||||
newViseme.label[1] = 'X';
|
||||
newViseme.label[2] = 'X';
|
||||
}
|
||||
newViseme.label[3] = '\0';
|
||||
}
|
||||
|
||||
// Set motor positions
|
||||
newViseme.motorPositions = positions;
|
||||
|
||||
visemes.push_back(newViseme);
|
||||
|
||||
// Update controlled motors list
|
||||
for (const auto& pos : positions) {
|
||||
addMotor(pos.motorID);
|
||||
}
|
||||
|
||||
// Update nextVisemeID if needed
|
||||
if (visemeID >= nextVisemeID) {
|
||||
nextVisemeID = visemeID + 1;
|
||||
}
|
||||
|
||||
Serial.print("[Viseme] Created viseme ID ");
|
||||
Serial.print(visemeID);
|
||||
Serial.print(" label '");
|
||||
Serial.print(newViseme.label);
|
||||
Serial.print("' with ");
|
||||
Serial.print(positions.size());
|
||||
Serial.println(" motors");
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
bool VisemeBehavior::triggerViseme(uint8_t visemeID) {
|
||||
Viseme* viseme = findViseme(visemeID);
|
||||
if (!viseme) {
|
||||
Serial.print("[Viseme] Unknown viseme ID ");
|
||||
Serial.println(visemeID);
|
||||
return false;
|
||||
}
|
||||
|
||||
// Copy positions for this viseme
|
||||
currentPositions = viseme->motorPositions;
|
||||
|
||||
// Activate and reset timer
|
||||
isActive = true;
|
||||
lastTriggerTime = millis();
|
||||
|
||||
Serial.print("[Viseme] Triggered '");
|
||||
Serial.print(viseme->label);
|
||||
Serial.print("' (ID ");
|
||||
Serial.print(visemeID);
|
||||
Serial.println(")");
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool VisemeBehavior::update() {
|
||||
if (!isActive) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Check for timeout
|
||||
unsigned long now = millis();
|
||||
if (now - lastTriggerTime >= TIMEOUT_MS) {
|
||||
// Timeout reached - deactivate
|
||||
isActive = false;
|
||||
currentPositions.clear();
|
||||
Serial.println("[Viseme] Timeout - deactivated");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool VisemeBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
|
||||
if (!isActive) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Look up motor in current positions
|
||||
for (const auto& pos : currentPositions) {
|
||||
if (pos.motorID == motorID) {
|
||||
position = pos.position;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Behavior Manager Implementation
|
||||
// ============================================================================
|
||||
|
||||
BehaviorManager behaviorManager;
|
||||
VisemeBehavior visemeBehavior;
|
||||
|
||||
BehaviorManager::BehaviorManager() {
|
||||
behaviors.clear();
|
||||
// Initialize all enabled states to false
|
||||
for (int i = 0; i < 256; i++) {
|
||||
enabledStates[i] = false;
|
||||
}
|
||||
}
|
||||
|
||||
void BehaviorManager::addBehavior(BehaviorID id, Behavior* behavior) {
|
||||
if (behavior == nullptr) return;
|
||||
|
||||
// Check if already added
|
||||
for (const auto& entry : behaviors) {
|
||||
if (entry.behavior == behavior || entry.id == id) return;
|
||||
}
|
||||
|
||||
behaviors.push_back({id, behavior});
|
||||
// New behaviors are enabled by default
|
||||
enabledStates[id] = true;
|
||||
}
|
||||
|
||||
void BehaviorManager::removeBehavior(Behavior* behavior) {
|
||||
behaviors.erase(
|
||||
std::remove_if(behaviors.begin(), behaviors.end(),
|
||||
[behavior](const BehaviorEntry& entry) {
|
||||
return entry.behavior == behavior;
|
||||
}),
|
||||
behaviors.end()
|
||||
);
|
||||
}
|
||||
|
||||
void BehaviorManager::setBehaviorEnabled(BehaviorID id, bool enabled) {
|
||||
enabledStates[id] = enabled;
|
||||
}
|
||||
|
||||
bool BehaviorManager::isBehaviorEnabled(BehaviorID id) const {
|
||||
return enabledStates[id];
|
||||
}
|
||||
|
||||
uint8_t BehaviorManager::getBehaviorCount() const {
|
||||
return behaviors.size();
|
||||
}
|
||||
|
||||
bool BehaviorManager::getBehaviorInfo(uint8_t index, BehaviorID& id, bool& enabled) const {
|
||||
if (index >= behaviors.size()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
id = behaviors[index].id;
|
||||
enabled = enabledStates[id];
|
||||
return true;
|
||||
}
|
||||
|
||||
void BehaviorManager::update() {
|
||||
// Update all enabled behaviors
|
||||
for (const auto& entry : behaviors) {
|
||||
if (entry.behavior && enabledStates[entry.id]) {
|
||||
entry.behavior->update();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool BehaviorManager::getMotorPosition(uint8_t motorID, uint16_t& position) {
|
||||
// Check all enabled behaviors to see if any wants to control this motor
|
||||
for (const auto& entry : behaviors) {
|
||||
if (entry.behavior && enabledStates[entry.id] &&
|
||||
entry.behavior->getMotorPosition(motorID, position)) {
|
||||
return true; // Found an enabled behavior controlling this motor
|
||||
}
|
||||
}
|
||||
return false; // No enabled behavior controlling this motor
|
||||
}
|
||||
265
behaviors.h
265
behaviors.h
|
|
@ -1,265 +0,0 @@
|
|||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include <vector>
|
||||
#include "sensors.h"
|
||||
#include "robotconfig.h"
|
||||
#include "noise.h"
|
||||
|
||||
// ============================================================================
|
||||
// Behavior IDs
|
||||
// ============================================================================
|
||||
|
||||
enum BehaviorID : uint8_t {
|
||||
BEHAVIOR_FOCUS = 1, // Focus behavior (radar tracking)
|
||||
BEHAVIOR_IDLE = 2, // Idle behavior (perlin noise for all motors)
|
||||
BEHAVIOR_VISEME = 3, // Viseme behavior (mouth motor positions)
|
||||
};
|
||||
|
||||
// ============================================================================
|
||||
// Base Behavior Class
|
||||
// ============================================================================
|
||||
|
||||
class Behavior {
|
||||
public:
|
||||
Behavior();
|
||||
virtual ~Behavior() = default;
|
||||
|
||||
// Get list of motor IDs this behavior controls
|
||||
const std::vector<uint8_t>& getControlledMotors() const { return controlledMotors; }
|
||||
|
||||
// Add a motor to the controlled list
|
||||
void addMotor(uint8_t motorID);
|
||||
|
||||
// Remove a motor from the controlled list
|
||||
void removeMotor(uint8_t motorID);
|
||||
|
||||
// Clear all controlled motors
|
||||
void clearMotors();
|
||||
|
||||
// Virtual method to update the behavior (called each frame)
|
||||
// Returns true if the behavior is active and wants to control motors
|
||||
virtual bool update() = 0;
|
||||
|
||||
// Virtual method to get the desired position for a motor
|
||||
// Returns true if this behavior wants to control this motor, false otherwise
|
||||
virtual bool getMotorPosition(uint8_t motorID, uint16_t& position) = 0;
|
||||
|
||||
protected:
|
||||
std::vector<uint8_t> controlledMotors;
|
||||
};
|
||||
|
||||
// ============================================================================
|
||||
// Focus Behavior - Tracks radar targets with eyes/neck
|
||||
// ============================================================================
|
||||
|
||||
class FocusBehavior : public Behavior {
|
||||
public:
|
||||
FocusBehavior();
|
||||
|
||||
// Update behavior - check radar 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
|
||||
|
||||
// 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);
|
||||
};
|
||||
|
||||
// ============================================================================
|
||||
// Idle Behavior - Adds perlin noise to all motors for natural idle motion
|
||||
// ============================================================================
|
||||
|
||||
class IdleBehavior : public Behavior {
|
||||
public:
|
||||
IdleBehavior();
|
||||
|
||||
// Initialize with list of motor IDs to control
|
||||
void initMotors(const std::vector<uint8_t>& motorIDs);
|
||||
|
||||
// Update behavior - calculates new noise positions
|
||||
bool update() override;
|
||||
|
||||
// Get motor position for a controlled motor
|
||||
bool getMotorPosition(uint8_t motorID, uint16_t& position) override;
|
||||
|
||||
private:
|
||||
// Store current positions for each motor (indexed by motor ID)
|
||||
uint16_t motorPositions[256];
|
||||
|
||||
// Time offset for perlin noise animation
|
||||
unsigned long startTime;
|
||||
|
||||
// Configuration
|
||||
static constexpr uint16_t POSITION_CENTER = 2047;
|
||||
static constexpr uint16_t NOISE_RANGE = 100; // ±500 from center
|
||||
static constexpr float NOISE_SPEED = 0.000125f; // How fast noise evolves (slower = smoother, 4x slower)
|
||||
static constexpr uint16_t MOTOR_SEED_OFFSET = 100; // Seed offset between motors for variety
|
||||
};
|
||||
|
||||
// ============================================================================
|
||||
// Viseme Behavior - Controls mouth motors for speech
|
||||
// ============================================================================
|
||||
|
||||
// Motor position within a viseme
|
||||
struct VisemeMotorPosition {
|
||||
uint8_t motorID;
|
||||
uint16_t position;
|
||||
};
|
||||
|
||||
// Viseme definition: ID, label (3 chars), and motor positions
|
||||
struct Viseme {
|
||||
uint8_t id;
|
||||
char label[4]; // 3 characters + null terminator
|
||||
std::vector<VisemeMotorPosition> motorPositions;
|
||||
};
|
||||
|
||||
class VisemeBehavior : public Behavior {
|
||||
public:
|
||||
VisemeBehavior();
|
||||
|
||||
// Add a viseme with a 3-char label (auto-assigns ID)
|
||||
// Returns the assigned viseme ID
|
||||
uint8_t addViseme(const char* label);
|
||||
|
||||
// Legacy: Add a viseme with specific ID and motor positions (for backwards compatibility)
|
||||
void addViseme(uint8_t id, uint16_t pos40, uint16_t pos43, uint16_t pos44);
|
||||
|
||||
// Add a viseme with specific ID, label, and motor positions
|
||||
void addViseme(uint8_t id, const char* label, uint16_t pos40, uint16_t pos43, uint16_t pos44);
|
||||
|
||||
// Delete a viseme by ID
|
||||
// Returns true if deleted, false if not found
|
||||
bool deleteViseme(uint8_t visemeID);
|
||||
|
||||
// Set motor positions for a viseme
|
||||
// Returns true if viseme found and updated, false otherwise
|
||||
bool setVisemeMotors(uint8_t visemeID, const std::vector<VisemeMotorPosition>& positions);
|
||||
|
||||
// Set motor positions and label for a viseme
|
||||
// Returns true if viseme found and updated, false otherwise
|
||||
bool setVisemeMotorsAndLabel(uint8_t visemeID, const char* label, const std::vector<VisemeMotorPosition>& positions);
|
||||
|
||||
// Create or update a viseme with specific ID, label, and motor positions
|
||||
// Returns true on success
|
||||
bool createOrUpdateViseme(uint8_t visemeID, const char* label, const std::vector<VisemeMotorPosition>& positions);
|
||||
|
||||
// Get all visemes (for VLST command)
|
||||
const std::vector<Viseme>& getVisemes() const { return visemes; }
|
||||
|
||||
// Trigger a viseme by ID - activates the behavior and sets positions
|
||||
// Returns true if viseme was found, false otherwise
|
||||
bool triggerViseme(uint8_t visemeID);
|
||||
|
||||
// Update behavior - checks for timeout and deactivates
|
||||
bool update() override;
|
||||
|
||||
// Get motor position for a controlled motor
|
||||
bool getMotorPosition(uint8_t motorID, uint16_t& position) override;
|
||||
|
||||
private:
|
||||
bool isActive;
|
||||
unsigned long lastTriggerTime;
|
||||
uint8_t nextVisemeID; // Auto-increment ID for new visemes
|
||||
|
||||
// Current active motor positions (when triggered)
|
||||
std::vector<VisemeMotorPosition> currentPositions;
|
||||
|
||||
// Registered visemes
|
||||
std::vector<Viseme> visemes;
|
||||
|
||||
// Configuration
|
||||
static constexpr unsigned long TIMEOUT_MS = 3000; // 3 second timeout
|
||||
static constexpr uint16_t DEFAULT_POSITION = 2047; // Center/rest position
|
||||
|
||||
// Helper to find viseme by ID
|
||||
Viseme* findViseme(uint8_t id);
|
||||
};
|
||||
|
||||
// ============================================================================
|
||||
// Behavior Manager - Manages active behaviors and resolves motor conflicts
|
||||
// ============================================================================
|
||||
|
||||
class BehaviorManager {
|
||||
public:
|
||||
BehaviorManager();
|
||||
|
||||
// Add a behavior to the manager with an ID
|
||||
void addBehavior(BehaviorID id, Behavior* behavior);
|
||||
|
||||
// Remove a behavior from the manager
|
||||
void removeBehavior(Behavior* behavior);
|
||||
|
||||
// Enable/disable a behavior by ID
|
||||
void setBehaviorEnabled(BehaviorID id, bool enabled);
|
||||
|
||||
// Check if a behavior is enabled
|
||||
bool isBehaviorEnabled(BehaviorID id) const;
|
||||
|
||||
// Get count of registered behaviors
|
||||
uint8_t getBehaviorCount() const;
|
||||
|
||||
// Get behavior info at index (for iteration)
|
||||
// Returns true if index is valid and fills out id and enabled
|
||||
bool getBehaviorInfo(uint8_t index, BehaviorID& id, bool& enabled) const;
|
||||
|
||||
// Update all enabled behaviors (call each frame)
|
||||
void update();
|
||||
|
||||
// Check if a behavior wants to control a specific motor
|
||||
// Returns true if a behavior provides a position, false otherwise
|
||||
bool getMotorPosition(uint8_t motorID, uint16_t& position);
|
||||
|
||||
private:
|
||||
struct BehaviorEntry {
|
||||
BehaviorID id;
|
||||
Behavior* behavior;
|
||||
};
|
||||
|
||||
std::vector<BehaviorEntry> behaviors;
|
||||
bool enabledStates[256] = {false}; // Track enabled state by ID
|
||||
};
|
||||
|
||||
// Global behavior manager instance
|
||||
extern BehaviorManager behaviorManager;
|
||||
|
||||
// Global viseme behavior instance (for command access)
|
||||
extern VisemeBehavior visemeBehavior;
|
||||
958
commands.cpp
958
commands.cpp
|
|
@ -1,958 +0,0 @@
|
|||
#include "commands.h"
|
||||
#include "nodegraph.h"
|
||||
#include "sensors.h"
|
||||
#include "behaviors.h"
|
||||
#include "esp_system.h"
|
||||
#include "soc/rtc_cntl_reg.h"
|
||||
#include <vector>
|
||||
#include <unordered_map>
|
||||
|
||||
// External references
|
||||
extern RobotConfig config;
|
||||
extern ServoManager servoManager;
|
||||
|
||||
// Global state instances
|
||||
AnimationState animState;
|
||||
MotorStreamState motorStream;
|
||||
|
||||
// ============================================================================
|
||||
// Chunked File Save Session (FSAV)
|
||||
// ============================================================================
|
||||
struct SaveSession {
|
||||
bool active = false;
|
||||
uint16_t totalChunks = 0;
|
||||
uint16_t receivedChunks = 0;
|
||||
uint32_t totalSize = 0;
|
||||
uint16_t chunkSize = 0; // size of non-final chunks
|
||||
std::vector<uint8_t> buffer;
|
||||
std::vector<bool> received;
|
||||
} g_save;
|
||||
|
||||
// Helper: reset save session
|
||||
static void resetSaveSession() {
|
||||
g_save.active = false;
|
||||
g_save.totalChunks = 0;
|
||||
g_save.receivedChunks = 0;
|
||||
g_save.totalSize = 0;
|
||||
g_save.chunkSize = 0;
|
||||
g_save.buffer.clear();
|
||||
g_save.received.clear();
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// AnimationState
|
||||
// ============================================================================
|
||||
|
||||
void AnimationState::play(PlayMode mode, uint8_t repeats, uint16_t startFrame) {
|
||||
current = &animation;
|
||||
current->setActive(true);
|
||||
playMode = mode;
|
||||
repeatsRemaining = repeats;
|
||||
this->startFrame = startFrame;
|
||||
playGeneration++; // Signal that a new animation has started
|
||||
}
|
||||
|
||||
void AnimationState::stop() {
|
||||
if (current) {
|
||||
current->setActive(false);
|
||||
}
|
||||
playMode = PLAY_IDLE;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// MotorStreamState
|
||||
// ============================================================================
|
||||
|
||||
void MotorStreamState::start() {
|
||||
active = true;
|
||||
lastStreamTime = millis();
|
||||
}
|
||||
|
||||
void MotorStreamState::stop() {
|
||||
active = false;
|
||||
}
|
||||
|
||||
bool MotorStreamState::shouldStream() {
|
||||
if (!active) return false;
|
||||
if (millis() - lastStreamTime >= STREAM_INTERVAL_MS) {
|
||||
lastStreamTime = millis();
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Command Dispatcher
|
||||
// ============================================================================
|
||||
|
||||
void dispatchCommand() {
|
||||
const char* tag = getReceivedTag();
|
||||
const uint8_t* payload = getReceivedPayload();
|
||||
uint16_t len = getReceivedPayloadLen();
|
||||
|
||||
// Identity & Config
|
||||
if (tagMatches(tag, Tag::IDENT)) {
|
||||
handleIdent(payload, len);
|
||||
}
|
||||
else if (tagMatches(tag, Tag::CONFG)) {
|
||||
handleConfig(payload, len);
|
||||
}
|
||||
// File Operations
|
||||
else if (tagMatches(tag, Tag::FLIST)) {
|
||||
handleFileList(payload, len);
|
||||
}
|
||||
else if (tagMatches(tag, Tag::FLOAD)) {
|
||||
handleFileLoad(payload, len);
|
||||
}
|
||||
else if (tagMatches(tag, Tag::FSAVE)) {
|
||||
handleFileSave(payload, len);
|
||||
}
|
||||
else if (tagMatches(tag, Tag::FDELE)) {
|
||||
handleFileDelete(payload, len);
|
||||
}
|
||||
else if (tagMatches(tag, Tag::FPLAY)) {
|
||||
handleFilePlay(payload, len);
|
||||
}
|
||||
else if (tagMatches(tag, Tag::FSTP)) {
|
||||
handleFileStop(payload, len);
|
||||
}
|
||||
// Motor Control
|
||||
else if (tagMatches(tag, Tag::MSET)) {
|
||||
handleMotorSet(payload, len);
|
||||
}
|
||||
else if (tagMatches(tag, Tag::MSCAN)) {
|
||||
handleMotorScan(payload, len);
|
||||
}
|
||||
else if (tagMatches(tag, Tag::MWRIT)) {
|
||||
handleMotorWrite(payload, len);
|
||||
}
|
||||
else if (tagMatches(tag, Tag::MSTRM)) {
|
||||
handleMotorStream(payload, len);
|
||||
}
|
||||
// Behaviors
|
||||
else if (tagMatches(tag, Tag::BHVR)) {
|
||||
handleBehavior(payload, len);
|
||||
}
|
||||
else if (tagMatches(tag, Tag::BLST)) {
|
||||
handleBehaviorList(payload, len);
|
||||
}
|
||||
// Visemes
|
||||
else if (tagMatches(tag, Tag::VLST)) {
|
||||
handleVisemeList(payload, len);
|
||||
}
|
||||
else if (tagMatches(tag, Tag::VADD)) {
|
||||
handleVisemeAdd(payload, len);
|
||||
}
|
||||
else if (tagMatches(tag, Tag::VDEL)) {
|
||||
handleVisemeDelete(payload, len);
|
||||
}
|
||||
else if (tagMatches(tag, Tag::VSET)) {
|
||||
handleVisemeSet(payload, len);
|
||||
}
|
||||
else if (tagMatches(tag, Tag::VSME)) {
|
||||
handleVisemeTrigger(payload, len);
|
||||
}
|
||||
// System
|
||||
else if (tagMatches(tag, Tag::BOOT)) {
|
||||
handleBootloader(payload, len);
|
||||
}
|
||||
else {
|
||||
char tagStr[5] = {tag[0], tag[1], tag[2], tag[3], 0};
|
||||
sendMessage("Unknown tag: " + String(tagStr));
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Identity & Config Handlers
|
||||
// ============================================================================
|
||||
|
||||
void handleIdent(const uint8_t* payload, uint16_t len) {
|
||||
std::vector<uint8_t> response = config.serializeToBytes();
|
||||
sendPacket(Tag::IDENT, response.data(), response.size());
|
||||
}
|
||||
|
||||
void handleConfig(const uint8_t* payload, uint16_t len) {
|
||||
RobotConfig newConfig;
|
||||
uint16_t offset = 0;
|
||||
|
||||
// Decode robot name
|
||||
if (offset >= len) { sendNack(Tag::CONFG, "Missing name"); return; }
|
||||
uint8_t nameLength = payload[offset++];
|
||||
for (uint8_t i = 0; i < nameLength && offset < len; ++i) {
|
||||
newConfig.deviceName += (char)payload[offset++];
|
||||
}
|
||||
|
||||
// Decode firmware version
|
||||
if (offset + 2 > len) { sendNack(Tag::CONFG, "Missing version"); return; }
|
||||
newConfig.firmwareVersion.major = payload[offset++];
|
||||
newConfig.firmwareVersion.minor = payload[offset++];
|
||||
|
||||
// Decode motor count
|
||||
if (offset >= len) { sendNack(Tag::CONFG, "Missing motors"); return; }
|
||||
uint8_t motorCount = payload[offset++];
|
||||
|
||||
// Decode motors
|
||||
for (uint8_t i = 0; i < motorCount && offset < len; ++i) {
|
||||
if (offset + 5 > len) break;
|
||||
|
||||
ServoModel model;
|
||||
model.major = payload[offset++];
|
||||
model.minor = payload[offset++];
|
||||
|
||||
uint16_t motorID = payload[offset++] | (payload[offset++] << 8);
|
||||
|
||||
uint8_t motorNameLength = payload[offset++];
|
||||
String motorName = "";
|
||||
for (uint8_t j = 0; j < motorNameLength && offset < len; ++j) {
|
||||
motorName += (char)payload[offset++];
|
||||
}
|
||||
|
||||
Motor motor;
|
||||
motor.motorID = motorID;
|
||||
motor.servoModel = model;
|
||||
motor.name = motorName;
|
||||
motor.position = 0;
|
||||
motor.isEnabled = true;
|
||||
|
||||
newConfig.motors.push_back(motor);
|
||||
}
|
||||
|
||||
config = newConfig;
|
||||
config.saveToFFat();
|
||||
|
||||
sendAck(Tag::CONFG);
|
||||
sendMessage("Config updated: " + config.deviceName);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// File Operation Handlers
|
||||
// ============================================================================
|
||||
|
||||
void handleFileList(const uint8_t* payload, uint16_t len) {
|
||||
File root = FFat.open("/");
|
||||
if (!root || !root.isDirectory()) {
|
||||
sendPacket(Tag::FLIST, nullptr, 0);
|
||||
return;
|
||||
}
|
||||
|
||||
String fileList = "";
|
||||
File file = root.openNextFile();
|
||||
while (file) {
|
||||
if (!file.isDirectory()) {
|
||||
fileList += String(file.name()) + "\n";
|
||||
}
|
||||
file = root.openNextFile();
|
||||
}
|
||||
|
||||
sendPacket(Tag::FLIST, (const uint8_t*)fileList.c_str(), fileList.length());
|
||||
}
|
||||
|
||||
void handleFileLoad(const uint8_t* payload, uint16_t len) {
|
||||
if (len == 0 || len >= 128) {
|
||||
sendNack(Tag::FLOAD, "Invalid filename");
|
||||
return;
|
||||
}
|
||||
|
||||
char filename[128];
|
||||
memcpy(filename, payload, len);
|
||||
filename[len] = '\0';
|
||||
|
||||
File file = FFat.open(filename, "r");
|
||||
if (!file || !file.available()) {
|
||||
sendNack(Tag::FLOAD, "File not found");
|
||||
return;
|
||||
}
|
||||
|
||||
size_t fileSize = file.size();
|
||||
const uint16_t CHUNK_SIZE = 1024; // match sender chunking
|
||||
uint16_t totalChunks = (fileSize + CHUNK_SIZE - 1) / CHUNK_SIZE;
|
||||
|
||||
// Prepare static header fields
|
||||
uint8_t header[8];
|
||||
header[0] = totalChunks & 0xFF;
|
||||
header[1] = (totalChunks >> 8) & 0xFF;
|
||||
header[4] = fileSize & 0xFF;
|
||||
header[5] = (fileSize >> 8) & 0xFF;
|
||||
header[6] = (fileSize >> 16) & 0xFF;
|
||||
header[7] = (fileSize >> 24) & 0xFF;
|
||||
|
||||
for (uint16_t chunkIndex = 0; chunkIndex < totalChunks; chunkIndex++) {
|
||||
size_t offset = (size_t)chunkIndex * CHUNK_SIZE;
|
||||
uint16_t thisChunk = (uint16_t)min((size_t)CHUNK_SIZE, fileSize - offset);
|
||||
|
||||
// Fill per-chunk fields
|
||||
header[2] = chunkIndex & 0xFF;
|
||||
header[3] = (chunkIndex >> 8) & 0xFF;
|
||||
|
||||
// Build payload: header + chunkData
|
||||
std::vector<uint8_t> payloadBuf;
|
||||
payloadBuf.reserve(8 + thisChunk);
|
||||
payloadBuf.insert(payloadBuf.end(), header, header + 8);
|
||||
|
||||
// Read chunk data directly into payload buffer
|
||||
size_t startIdx = payloadBuf.size();
|
||||
payloadBuf.resize(startIdx + thisChunk);
|
||||
file.seek(offset);
|
||||
file.read(payloadBuf.data() + startIdx, thisChunk);
|
||||
|
||||
sendPacket(Tag::FLOAD, payloadBuf.data(), payloadBuf.size());
|
||||
delay(2); // small pacing to avoid overwhelming host
|
||||
}
|
||||
|
||||
file.close();
|
||||
}
|
||||
|
||||
void handleFileSave(const uint8_t* payload, uint16_t len) {
|
||||
// Chunked protocol:
|
||||
// [totalChunks:2][chunkIndex:2][totalSize:4][chunkData:...]
|
||||
if (len < 8) {
|
||||
sendNack(Tag::FSAVE, "Payload too short");
|
||||
return;
|
||||
}
|
||||
|
||||
uint16_t totalChunks = payload[0] | (payload[1] << 8);
|
||||
uint16_t chunkIndex = payload[2] | (payload[3] << 8);
|
||||
uint32_t totalSize = payload[4] | (payload[5] << 8) | (payload[6] << 16) | (payload[7] << 24);
|
||||
const uint8_t* chunkData = payload + 8;
|
||||
uint16_t chunkLen = len - 8;
|
||||
|
||||
// Basic validation
|
||||
if (totalChunks == 0) {
|
||||
sendNack(Tag::FSAVE, "totalChunks=0");
|
||||
return;
|
||||
}
|
||||
if (chunkIndex >= totalChunks) {
|
||||
sendNack(Tag::FSAVE, "chunkIndex out of range");
|
||||
return;
|
||||
}
|
||||
if (totalSize == 0) {
|
||||
sendNack(Tag::FSAVE, "totalSize=0");
|
||||
return;
|
||||
}
|
||||
|
||||
// Start new session if needed
|
||||
if (!g_save.active || g_save.totalSize != totalSize || g_save.totalChunks != totalChunks) {
|
||||
g_save.active = true;
|
||||
g_save.totalChunks = totalChunks;
|
||||
g_save.receivedChunks = 0;
|
||||
g_save.totalSize = totalSize;
|
||||
g_save.chunkSize = chunkLen; // assume first chunk size is the standard chunk size
|
||||
g_save.buffer.assign(totalSize, 0);
|
||||
g_save.received.assign(totalChunks, false);
|
||||
}
|
||||
|
||||
// Calculate offset
|
||||
uint32_t offset = (uint32_t)chunkIndex * (uint32_t)g_save.chunkSize;
|
||||
// For safety, allow last chunk to be smaller
|
||||
if (offset + chunkLen > g_save.totalSize) {
|
||||
sendNack(Tag::FSAVE, "chunk overflow");
|
||||
return;
|
||||
}
|
||||
|
||||
// If not already received, copy in
|
||||
if (!g_save.received[chunkIndex]) {
|
||||
memcpy(g_save.buffer.data() + offset, chunkData, chunkLen);
|
||||
g_save.received[chunkIndex] = true;
|
||||
g_save.receivedChunks++;
|
||||
}
|
||||
|
||||
// If not all chunks yet, ACK chunk and return
|
||||
if (g_save.receivedChunks < g_save.totalChunks) {
|
||||
sendAck(Tag::FSAVE); // per-chunk ACK
|
||||
return;
|
||||
}
|
||||
|
||||
// All chunks received - parse and save animation
|
||||
bool ok = parseAndSaveAnimation(g_save.buffer.data(), g_save.buffer.size(), animState.animation);
|
||||
g_save.active = false;
|
||||
g_save.buffer.clear();
|
||||
g_save.received.clear();
|
||||
|
||||
if (ok) {
|
||||
sendAck(Tag::FSAVE);
|
||||
} else {
|
||||
sendNack(Tag::FSAVE, "Parse failed");
|
||||
}
|
||||
}
|
||||
|
||||
void handleFileDelete(const uint8_t* payload, uint16_t len) {
|
||||
if (len < 2) {
|
||||
sendNack(Tag::FDELE, "Invalid request");
|
||||
return;
|
||||
}
|
||||
|
||||
uint16_t filenameLen = payload[0] | (payload[1] << 8);
|
||||
if (len < 2 + filenameLen) {
|
||||
sendNack(Tag::FDELE, "Invalid filename");
|
||||
return;
|
||||
}
|
||||
|
||||
char filename[128];
|
||||
memcpy(filename, payload + 2, min((uint16_t)127, filenameLen));
|
||||
filename[min((uint16_t)127, filenameLen)] = '\0';
|
||||
|
||||
String fullPath = "/" + String(filename);
|
||||
deleteFile(FFat, fullPath.c_str());
|
||||
|
||||
sendAck(Tag::FDELE);
|
||||
sendMessage("Deleted: " + String(filename));
|
||||
}
|
||||
|
||||
void handleFilePlay(const uint8_t* payload, uint16_t len) {
|
||||
if (len < 6) { // Minimum: filenameLen(2) + filename(1) + mode(1) + repeats(1) + startFrame(2)
|
||||
sendNack(Tag::FPLAY, "Invalid request");
|
||||
return;
|
||||
}
|
||||
|
||||
uint16_t filenameLen = payload[0] | (payload[1] << 8);
|
||||
if (len < 2 + filenameLen + 4) { // filenameLen + filename + mode + repeats + startFrame
|
||||
sendNack(Tag::FPLAY, "Invalid payload length");
|
||||
return;
|
||||
}
|
||||
|
||||
char filename[128];
|
||||
memcpy(filename, payload + 2, min((uint16_t)127, filenameLen));
|
||||
filename[min((uint16_t)127, filenameLen)] = '\0';
|
||||
|
||||
uint16_t offset = 2 + filenameLen;
|
||||
PlayMode mode = static_cast<PlayMode>(payload[offset]);
|
||||
uint8_t repeats = payload[offset + 1];
|
||||
uint16_t startFrame = payload[offset + 2] | (payload[offset + 3] << 8);
|
||||
|
||||
// Debug: show parsed startFrame
|
||||
sendMessage("FPLAY parsed - startFrame bytes: [" + String(payload[offset + 2]) + ", " + String(payload[offset + 3]) + "] = " + String(startFrame));
|
||||
|
||||
animState.animation.clear();
|
||||
String fullPath = "/" + String(filename);
|
||||
|
||||
if (animState.animation.loadFromFile(fullPath.c_str())) {
|
||||
animState.play(mode, repeats, startFrame);
|
||||
sendAck(Tag::FPLAY);
|
||||
sendMessage("Playing: " + String(filename) + " from frame " + String(startFrame));
|
||||
sendMessage("animState.startFrame stored as: " + String(animState.startFrame));
|
||||
} else {
|
||||
sendNack(Tag::FPLAY, "Load failed");
|
||||
}
|
||||
}
|
||||
|
||||
void handleFileStop(const uint8_t* payload, uint16_t len) {
|
||||
// FSTP has no payload (len should be 0, but we don't strictly require it)
|
||||
animState.stop();
|
||||
sendAck(Tag::FSTP);
|
||||
sendMessage("Animation stopped");
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Behavior Handler
|
||||
// ============================================================================
|
||||
|
||||
void handleBehavior(const uint8_t* payload, uint16_t len) {
|
||||
// BHVR payload: [behaviorID (1 byte)][enable (1 byte)]
|
||||
if (len < 2) {
|
||||
sendNack(Tag::BHVR, "Invalid payload length");
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t behaviorID = payload[0];
|
||||
uint8_t enable = payload[1];
|
||||
|
||||
// Validate behavior ID
|
||||
if (behaviorID == 0 || behaviorID > 255) {
|
||||
sendNack(Tag::BHVR, "Invalid behavior ID");
|
||||
return;
|
||||
}
|
||||
|
||||
// Enable/disable the behavior
|
||||
bool enabled = (enable != 0);
|
||||
behaviorManager.setBehaviorEnabled(static_cast<BehaviorID>(behaviorID), enabled);
|
||||
|
||||
// Save config to persist the behavior state change
|
||||
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
|
||||
|
||||
// Send acknowledgment
|
||||
sendAck(Tag::BHVR);
|
||||
|
||||
// Send status message
|
||||
String status = enabled ? "enabled" : "disabled";
|
||||
sendMessage("Behavior " + String(behaviorID) + " " + status);
|
||||
}
|
||||
|
||||
void handleBehaviorList(const uint8_t* payload, uint16_t len) {
|
||||
// BLST payload: none (can be empty)
|
||||
// Response: [count (1 byte)][behaviorID1 (1 byte)][enabled1 (1 byte)]...
|
||||
|
||||
uint8_t count = behaviorManager.getBehaviorCount();
|
||||
|
||||
// Build response payload: [count][id1][enabled1][id2][enabled2]...
|
||||
std::vector<uint8_t> response;
|
||||
response.push_back(count);
|
||||
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
BehaviorID id;
|
||||
bool enabled;
|
||||
if (behaviorManager.getBehaviorInfo(i, id, enabled)) {
|
||||
response.push_back(static_cast<uint8_t>(id));
|
||||
response.push_back(enabled ? 1 : 0);
|
||||
}
|
||||
}
|
||||
|
||||
// Send response packet
|
||||
sendPacket(Tag::BLST, response.data(), response.size());
|
||||
|
||||
// Also send debug message with behavior names
|
||||
String msg = "Behaviors: ";
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
BehaviorID id;
|
||||
bool enabled;
|
||||
if (behaviorManager.getBehaviorInfo(i, id, enabled)) {
|
||||
if (i > 0) msg += ", ";
|
||||
String name;
|
||||
switch (id) {
|
||||
case BEHAVIOR_FOCUS:
|
||||
name = "Focus";
|
||||
break;
|
||||
case BEHAVIOR_IDLE:
|
||||
name = "Idle";
|
||||
break;
|
||||
case BEHAVIOR_VISEME:
|
||||
name = "Viseme";
|
||||
break;
|
||||
default:
|
||||
name = "Unknown(" + String(static_cast<uint8_t>(id)) + ")";
|
||||
break;
|
||||
}
|
||||
msg += name + "(" + String(static_cast<uint8_t>(id)) + ")=";
|
||||
msg += enabled ? "ON" : "OFF";
|
||||
}
|
||||
}
|
||||
sendMessage(msg);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Viseme Handlers
|
||||
// ============================================================================
|
||||
|
||||
void handleVisemeList(const uint8_t* payload, uint16_t len) {
|
||||
// VLST - returns all visemes with their labels and motor positions
|
||||
const std::vector<Viseme>& visemes = visemeBehavior.getVisemes();
|
||||
|
||||
// Build response payload
|
||||
std::vector<uint8_t> response;
|
||||
response.push_back(visemes.size()); // count
|
||||
|
||||
for (const Viseme& v : visemes) {
|
||||
response.push_back(v.id); // viseme_id
|
||||
|
||||
// Label (3 bytes)
|
||||
response.push_back(v.label[0]);
|
||||
response.push_back(v.label[1]);
|
||||
response.push_back(v.label[2]);
|
||||
|
||||
// Motor positions
|
||||
response.push_back(v.motorPositions.size()); // motor_count
|
||||
for (const VisemeMotorPosition& mp : v.motorPositions) {
|
||||
response.push_back(mp.motorID);
|
||||
response.push_back(mp.position & 0xFF); // position_low
|
||||
response.push_back((mp.position >> 8) & 0xFF); // position_high
|
||||
}
|
||||
}
|
||||
|
||||
sendPacket(Tag::VLST, response.data(), response.size());
|
||||
}
|
||||
|
||||
void handleVisemeAdd(const uint8_t* payload, uint16_t len) {
|
||||
// VADD payload: [label: 3 bytes]
|
||||
if (len < 3) {
|
||||
sendNack(Tag::VADD, "Invalid payload length (need 3-byte label)");
|
||||
return;
|
||||
}
|
||||
|
||||
// Extract label (3 bytes)
|
||||
char label[4];
|
||||
label[0] = payload[0];
|
||||
label[1] = payload[1];
|
||||
label[2] = payload[2];
|
||||
label[3] = '\0';
|
||||
|
||||
// Add the viseme
|
||||
uint8_t newID = visemeBehavior.addViseme(label);
|
||||
|
||||
// Save config to persist the new viseme
|
||||
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
|
||||
|
||||
// Send ACK with the new ID
|
||||
uint8_t ackPayload[1] = { newID };
|
||||
sendPacket(Tag::ACK, ackPayload, 1);
|
||||
}
|
||||
|
||||
void handleVisemeDelete(const uint8_t* payload, uint16_t len) {
|
||||
// VDEL payload: [viseme_id: 1 byte]
|
||||
if (len < 1) {
|
||||
sendNack(Tag::VDEL, "Invalid payload length");
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t visemeID = payload[0];
|
||||
|
||||
if (visemeBehavior.deleteViseme(visemeID)) {
|
||||
// Save config to persist the deletion
|
||||
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
|
||||
sendAck(Tag::VDEL);
|
||||
} else {
|
||||
sendNack(Tag::VDEL, "Viseme not found");
|
||||
}
|
||||
}
|
||||
|
||||
void handleVisemeSet(const uint8_t* payload, uint16_t len) {
|
||||
// VSET payload: [viseme_id: 1][label: 3 bytes][motor_count: 1][motor_id: 1][pos_low: 1][pos_high: 1]...
|
||||
if (len < 5) { // Minimum: viseme_id(1) + label(3) + motor_count(1)
|
||||
sendNack(Tag::VSET, "Invalid payload length");
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t visemeID = payload[0];
|
||||
|
||||
// Extract label (3 bytes)
|
||||
char label[4];
|
||||
label[0] = payload[1];
|
||||
label[1] = payload[2];
|
||||
label[2] = payload[3];
|
||||
label[3] = '\0';
|
||||
|
||||
uint8_t motorCount = payload[4];
|
||||
|
||||
// Calculate expected length: viseme_id(1) + label(3) + motor_count(1) + motors(motorCount * 3)
|
||||
if (len < 5 + motorCount * 3) {
|
||||
sendNack(Tag::VSET, "Motor data truncated");
|
||||
return;
|
||||
}
|
||||
|
||||
// Parse motor positions
|
||||
std::vector<VisemeMotorPosition> positions;
|
||||
for (uint8_t i = 0; i < motorCount; i++) {
|
||||
uint16_t offset = 5 + i * 3; // Start after viseme_id(1) + label(3) + motor_count(1)
|
||||
VisemeMotorPosition mp;
|
||||
mp.motorID = payload[offset];
|
||||
mp.position = payload[offset + 1] | (payload[offset + 2] << 8); // Little-endian
|
||||
positions.push_back(mp);
|
||||
}
|
||||
|
||||
// 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);
|
||||
sendAck(Tag::VSET);
|
||||
} else {
|
||||
sendNack(Tag::VSET, "Failed to update viseme");
|
||||
}
|
||||
}
|
||||
|
||||
void handleVisemeTrigger(const uint8_t* payload, uint16_t len) {
|
||||
// VSME payload: [viseme_id: 1 byte]
|
||||
// Fire-and-forget - no response expected
|
||||
if (len < 1) {
|
||||
return; // Silent fail for fire-and-forget
|
||||
}
|
||||
|
||||
uint8_t visemeID = payload[0];
|
||||
visemeBehavior.triggerViseme(visemeID);
|
||||
// No response sent - fire and forget
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Motor Control Handlers
|
||||
// ============================================================================
|
||||
|
||||
void handleMotorSet(const uint8_t* payload, uint16_t len) {
|
||||
if (len % 3 != 0) {
|
||||
sendNack(Tag::MSET, "Invalid length");
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t motorCount = len / 3;
|
||||
uint8_t ids[motorCount];
|
||||
uint16_t positions[motorCount];
|
||||
uint16_t speeds[motorCount];
|
||||
|
||||
for (uint8_t i = 0; i < motorCount; ++i) {
|
||||
ids[i] = payload[i * 3];
|
||||
positions[i] = (payload[i * 3 + 2] << 8) | payload[i * 3 + 1];
|
||||
speeds[i] = 0;
|
||||
}
|
||||
|
||||
servoManager.syncWritePositions(ids, positions, speeds, motorCount, config, 0);
|
||||
sendAck(Tag::MSET);
|
||||
}
|
||||
|
||||
void handleMotorScan(const uint8_t* payload, uint16_t len) {
|
||||
if (len < 1) {
|
||||
sendNack(Tag::MSCAN, "Invalid request");
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t channel = payload[0];
|
||||
Feetech* servo = servoManager[channel];
|
||||
|
||||
for (int i = 0; i < 254; i++) {
|
||||
servo->sendPing(i);
|
||||
uint8_t val = servo->waitOnData1Byte(10);
|
||||
|
||||
if (val != 0) {
|
||||
uint8_t response[33];
|
||||
response[0] = channel;
|
||||
response[1] = i;
|
||||
|
||||
uint16_t model = servo->getModel(i);
|
||||
servo->setFeetechMode(model);
|
||||
|
||||
uint16_t minAngle = servo->getMinAngleLimit(i);
|
||||
uint16_t maxAngle = servo->getMaxAngleLimit(i);
|
||||
uint16_t position = servo->getPosition(i);
|
||||
uint8_t cwDead = servo->getCWDeadZone(i);
|
||||
uint8_t ccwDead = servo->getCCWDeadZone(i);
|
||||
uint16_t offset = servo->getOffset(i);
|
||||
uint8_t mode = servo->getMode(i);
|
||||
uint8_t torque = servo->getTorqueEnable(i);
|
||||
uint8_t accel = servo->getAcceleration(i);
|
||||
uint16_t goalPos = servo->getGoalPosition(i);
|
||||
uint16_t goalTime = servo->getGoalTime(i);
|
||||
uint16_t goalSpeed = servo->getGoalSpeed(i);
|
||||
uint8_t lock = servo->getLock(i);
|
||||
int16_t speed = servo->getSpeed(i);
|
||||
uint16_t load = servo->getLoad(i);
|
||||
uint8_t temp = servo->getTemperature(i);
|
||||
uint8_t moving = servo->getMoving(i);
|
||||
uint8_t current = servo->getCurrent(i);
|
||||
uint8_t voltage = servo->getVoltage(i);
|
||||
|
||||
// Pack response
|
||||
response[2] = (model >> 8) & 0xFF;
|
||||
response[3] = model & 0xFF;
|
||||
response[4] = (minAngle >> 8) & 0xFF;
|
||||
response[5] = minAngle & 0xFF;
|
||||
response[6] = (maxAngle >> 8) & 0xFF;
|
||||
response[7] = maxAngle & 0xFF;
|
||||
response[8] = (position >> 8) & 0xFF;
|
||||
response[9] = position & 0xFF;
|
||||
response[10] = cwDead;
|
||||
response[11] = ccwDead;
|
||||
response[12] = (offset >> 8) & 0xFF;
|
||||
response[13] = offset & 0xFF;
|
||||
response[14] = mode;
|
||||
response[15] = torque;
|
||||
response[16] = accel;
|
||||
response[17] = (goalPos >> 8) & 0xFF;
|
||||
response[18] = goalPos & 0xFF;
|
||||
response[19] = (goalTime >> 8) & 0xFF;
|
||||
response[20] = goalTime & 0xFF;
|
||||
response[21] = (goalSpeed >> 8) & 0xFF;
|
||||
response[22] = goalSpeed & 0xFF;
|
||||
response[23] = lock;
|
||||
response[24] = (speed >> 8) & 0xFF;
|
||||
response[25] = speed & 0xFF;
|
||||
response[26] = (load >> 8) & 0xFF;
|
||||
response[27] = load & 0xFF;
|
||||
response[28] = temp;
|
||||
response[29] = moving;
|
||||
response[30] = (current >> 8) & 0xFF;
|
||||
response[31] = current & 0xFF;
|
||||
response[32] = voltage;
|
||||
|
||||
sendPacket(Tag::MSCAN, response, 33);
|
||||
}
|
||||
}
|
||||
|
||||
// Signal end of scan
|
||||
uint8_t endResponse[2] = { channel, 255 };
|
||||
sendPacket(Tag::MSCAN, endResponse, 2);
|
||||
}
|
||||
|
||||
void handleMotorWrite(const uint8_t* payload, uint16_t len) {
|
||||
if (len < 5) {
|
||||
sendNack(Tag::MWRIT, "Invalid request");
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t channel = payload[0];
|
||||
uint8_t id = payload[1];
|
||||
uint8_t reg = payload[2];
|
||||
uint8_t dataLen = payload[3];
|
||||
|
||||
Feetech* servo = servoManager[channel];
|
||||
uint8_t model = servo->getMajorModel(id);
|
||||
servo->setFeetechMode(model);
|
||||
|
||||
// Special case for ID changes (reg 5)
|
||||
if (reg == 5 && dataLen == 1) {
|
||||
servo->changeID(id, payload[4]);
|
||||
sendAck(Tag::MWRIT);
|
||||
sendMessage("ID changed");
|
||||
return;
|
||||
}
|
||||
|
||||
if (dataLen == 1) {
|
||||
servo->write1Byte(id, reg, payload[4]);
|
||||
uint8_t response = servo->waitOnData1Byte(10);
|
||||
sendPacket(Tag::MWRIT, &response, 1);
|
||||
} else {
|
||||
servo->write2Bytes(id, reg, payload[4] | (payload[5] << 8));
|
||||
uint16_t response = servo->waitOnData2Bytes(10);
|
||||
uint8_t respBuf[2] = { (uint8_t)(response & 0xFF), (uint8_t)(response >> 8) };
|
||||
sendPacket(Tag::MWRIT, respBuf, 2);
|
||||
}
|
||||
}
|
||||
|
||||
void handleMotorStream(const uint8_t* payload, uint16_t len) {
|
||||
if (len < 1) {
|
||||
sendNack(Tag::MSTRM, "Invalid request");
|
||||
return;
|
||||
}
|
||||
|
||||
bool enable = payload[0] != 0;
|
||||
|
||||
if (enable) {
|
||||
// Disable torque for position reading
|
||||
for (const Motor& motor : config.motors) {
|
||||
servoManager[0]->setFeetechMode(motor.servoModel.major);
|
||||
servoManager[0]->disableTorque(motor.motorID);
|
||||
}
|
||||
motorStream.start();
|
||||
} else {
|
||||
motorStream.stop();
|
||||
}
|
||||
|
||||
sendAck(Tag::MSTRM);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// System Handlers
|
||||
// ============================================================================
|
||||
|
||||
void handleBootloader(const uint8_t* payload, uint16_t len) {
|
||||
sendMessage("Entering bootloader...");
|
||||
Serial.flush();
|
||||
delay(100);
|
||||
|
||||
REG_WRITE(RTC_CNTL_OPTION1_REG, RTC_CNTL_FORCE_DOWNLOAD_BOOT);
|
||||
esp_restart();
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Helper Functions
|
||||
// ============================================================================
|
||||
|
||||
void sendMotorPositions() {
|
||||
std::vector<uint8_t> payload;
|
||||
|
||||
for (const Motor& motor : config.motors) {
|
||||
payload.push_back(motor.motorID);
|
||||
payload.push_back(motor.position & 0xFF);
|
||||
payload.push_back((motor.position >> 8) & 0xFF);
|
||||
}
|
||||
|
||||
sendPacket(Tag::MPOS, payload.data(), payload.size());
|
||||
}
|
||||
|
||||
bool parseAndSaveAnimation(const uint8_t* payload, uint16_t len, Animation& animation) {
|
||||
const uint8_t* ptr = payload;
|
||||
uint16_t remaining = len;
|
||||
|
||||
// Optional filename block (only present if sender included it)
|
||||
String filename = "received.anim";
|
||||
if (remaining >= 4 && strncmp((const char*)ptr, "ANIM", 4) != 0) {
|
||||
if (remaining < 2) return false;
|
||||
uint16_t filenameLen = ptr[0] | (ptr[1] << 8);
|
||||
ptr += 2;
|
||||
remaining -= 2;
|
||||
if (filenameLen > 127 || remaining < filenameLen) return false;
|
||||
char fname[128];
|
||||
memcpy(fname, ptr, filenameLen);
|
||||
fname[filenameLen] = '\0';
|
||||
filename = fname;
|
||||
ptr += filenameLen;
|
||||
remaining -= filenameLen;
|
||||
}
|
||||
|
||||
// Header: 16 bytes
|
||||
if (remaining < 16) return false;
|
||||
memcpy(animation.header.magic, ptr, 4);
|
||||
if (strncmp(animation.header.magic, "ANIM", 4) != 0) {
|
||||
sendMessage("Invalid magic header");
|
||||
return false;
|
||||
}
|
||||
animation.header.frameCount = ptr[4] | (ptr[5] << 8);
|
||||
animation.header.version = ptr[6];
|
||||
animation.header.frameRate = ptr[7];
|
||||
memcpy(animation.header.reserved, ptr + 8, 8);
|
||||
ptr += 16;
|
||||
remaining -= 16;
|
||||
|
||||
if (animation.header.version == 2) {
|
||||
// Version 2: Frame data block
|
||||
// Calculate motor count from remaining data
|
||||
if (animation.header.frameCount == 0) return false;
|
||||
uint16_t frameDataSize = remaining;
|
||||
uint16_t frameSize = frameDataSize / animation.header.frameCount;
|
||||
uint16_t motorCount = frameSize / sizeof(MotorPosition);
|
||||
|
||||
if (frameSize % sizeof(MotorPosition) != 0) return false;
|
||||
if (frameDataSize < animation.header.frameCount * motorCount * sizeof(MotorPosition)) return false;
|
||||
|
||||
animation.clearFrameData();
|
||||
|
||||
// Read all frames
|
||||
for (uint16_t frameIndex = 0; frameIndex < animation.header.frameCount; frameIndex++) {
|
||||
std::vector<MotorPosition> frame;
|
||||
frame.reserve(motorCount);
|
||||
|
||||
for (uint16_t motorIndex = 0; motorIndex < motorCount; motorIndex++) {
|
||||
if (remaining < sizeof(MotorPosition)) return false;
|
||||
MotorPosition motorPos;
|
||||
memcpy(&motorPos, ptr, sizeof(MotorPosition));
|
||||
frame.push_back(motorPos);
|
||||
ptr += sizeof(MotorPosition);
|
||||
remaining -= sizeof(MotorPosition);
|
||||
}
|
||||
|
||||
animation.setFrameData(frameIndex, frame);
|
||||
}
|
||||
} else {
|
||||
// Version 1: Curve count (at start of curve block)
|
||||
if (remaining < 2) return false;
|
||||
uint16_t curveCount = ptr[0] | (ptr[1] << 8);
|
||||
ptr += 2;
|
||||
remaining -= 2;
|
||||
|
||||
// Curves (17 bytes each, packed)
|
||||
uint16_t curveDataSize = curveCount * sizeof(CurveSegment);
|
||||
if (remaining < curveDataSize) return false;
|
||||
animation.clearAllCurves();
|
||||
for (uint16_t i = 0; i < curveCount; i++) {
|
||||
CurveSegment seg;
|
||||
memcpy(&seg, ptr, sizeof(CurveSegment));
|
||||
animation.addCurveSegment(seg);
|
||||
ptr += sizeof(CurveSegment);
|
||||
}
|
||||
remaining -= curveDataSize;
|
||||
|
||||
// Node graph (whatever remains)
|
||||
if (remaining > 0) {
|
||||
loadNodeGraph(ptr, remaining, animation.nodeGraph);
|
||||
animation.nodeGraph.bindAnimationContext(&animation);
|
||||
}
|
||||
}
|
||||
|
||||
// Save to file
|
||||
String fullPath = "/" + filename;
|
||||
animation.saveToFile(fullPath.c_str());
|
||||
sendMessage("Saved: " + filename);
|
||||
return true;
|
||||
}
|
||||
|
||||
void deleteFile(fs::FS& fs, const char* path) {
|
||||
if (fs.remove(path)) {
|
||||
Serial.printf("Deleted: %s\n", path);
|
||||
} else {
|
||||
Serial.printf("Delete failed: %s\n", path);
|
||||
}
|
||||
}
|
||||
97
commands.h
97
commands.h
|
|
@ -1,97 +0,0 @@
|
|||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include <FFat.h>
|
||||
#include "protocol.h"
|
||||
#include "animation.h"
|
||||
#include "robotconfig.h"
|
||||
#include "motorcontrol.h"
|
||||
|
||||
// ============================================================================
|
||||
// Animation State
|
||||
// ============================================================================
|
||||
|
||||
struct AnimationState {
|
||||
Animation animation;
|
||||
Animation* current = nullptr;
|
||||
PlayMode playMode = PLAY_IDLE;
|
||||
uint8_t repeatsRemaining = 0;
|
||||
uint16_t startFrame = 0; // Frame to start playback from
|
||||
uint8_t playGeneration = 0; // Increments each time play() is called
|
||||
|
||||
void play(PlayMode mode, uint8_t repeats = 0, uint16_t startFrame = 0);
|
||||
void stop();
|
||||
};
|
||||
|
||||
extern AnimationState animState;
|
||||
|
||||
// ============================================================================
|
||||
// Motor Position Streaming State
|
||||
// ============================================================================
|
||||
|
||||
struct MotorStreamState {
|
||||
bool active = false;
|
||||
unsigned long lastStreamTime = 0;
|
||||
static constexpr unsigned long STREAM_INTERVAL_MS = 50;
|
||||
|
||||
void start();
|
||||
void stop();
|
||||
bool shouldStream();
|
||||
};
|
||||
|
||||
extern MotorStreamState motorStream;
|
||||
|
||||
// ============================================================================
|
||||
// Command Dispatcher
|
||||
// ============================================================================
|
||||
|
||||
// Process a received packet - call after receivePacket() returns true
|
||||
void dispatchCommand();
|
||||
|
||||
// ============================================================================
|
||||
// Individual Command Handlers
|
||||
// ============================================================================
|
||||
|
||||
// Identity & Config
|
||||
void handleIdent(const uint8_t* payload, uint16_t len);
|
||||
void handleConfig(const uint8_t* payload, uint16_t len);
|
||||
|
||||
// File Operations
|
||||
void handleFileList(const uint8_t* payload, uint16_t len);
|
||||
void handleFileLoad(const uint8_t* payload, uint16_t len);
|
||||
void handleFileSave(const uint8_t* payload, uint16_t len);
|
||||
void handleFileDelete(const uint8_t* payload, uint16_t len);
|
||||
void handleFilePlay(const uint8_t* payload, uint16_t len);
|
||||
void handleFileStop(const uint8_t* payload, uint16_t len);
|
||||
|
||||
// Motor Control
|
||||
void handleMotorSet(const uint8_t* payload, uint16_t len);
|
||||
void handleMotorScan(const uint8_t* payload, uint16_t len);
|
||||
void handleMotorWrite(const uint8_t* payload, uint16_t len);
|
||||
void handleMotorStream(const uint8_t* payload, uint16_t len);
|
||||
|
||||
// Behaviors
|
||||
void handleBehavior(const uint8_t* payload, uint16_t len);
|
||||
void handleBehaviorList(const uint8_t* payload, uint16_t len);
|
||||
|
||||
// Visemes
|
||||
void handleVisemeList(const uint8_t* payload, uint16_t len);
|
||||
void handleVisemeAdd(const uint8_t* payload, uint16_t len);
|
||||
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);
|
||||
|
||||
// System
|
||||
void handleBootloader(const uint8_t* payload, uint16_t len);
|
||||
|
||||
// ============================================================================
|
||||
// Helper Functions
|
||||
// ============================================================================
|
||||
|
||||
// Send current motor positions
|
||||
void sendMotorPositions();
|
||||
|
||||
// Parse animation from payload and save to file
|
||||
bool parseAndSaveAnimation(const uint8_t* payload, uint16_t len, Animation& animation);
|
||||
|
||||
// Delete a file
|
||||
void deleteFile(fs::FS& fs, const char* path);
|
||||
13
feetech.cpp
13
feetech.cpp
|
|
@ -377,16 +377,11 @@ uint8_t Feetech::getMoving(uint8_t id) {
|
|||
return waitOnData1Byte(10);
|
||||
}
|
||||
|
||||
// STS/SMS: actual current at 0x45, SCS: use load at 0x3C as proxy
|
||||
// Multiplier could be wrong
|
||||
uint16_t Feetech::getCurrent(uint8_t id) {
|
||||
if (feetechMode == MODE_STS || feetechMode == MODE_SMSA || feetechMode == MODE_SMSB) {
|
||||
sendRequest(id, REQUEST_CURRENT_CURRENT, 2);
|
||||
return waitOnData2Bytes(10);
|
||||
} else {
|
||||
// SCS doesn't have current register, use load as proxy
|
||||
sendRequest(id, REQUEST_CURRENT_LOAD, 2);
|
||||
return flipBytes(waitOnData2Bytes(10));
|
||||
}
|
||||
sendRequest(id, REQUEST_CURRENT_CURRENT, 2);
|
||||
//return waitOnData2Bytes(10) * 0.01; FLOAT
|
||||
return waitOnData2Bytes(10);
|
||||
}
|
||||
|
||||
void Feetech::sendRequest(uint8_t id, byte instruction, uint8_t byteCount) {
|
||||
|
|
|
|||
738
ls_firmware.ino
738
ls_firmware.ino
|
|
@ -1,738 +0,0 @@
|
|||
/**
|
||||
* HansonServo Firmware
|
||||
*
|
||||
* Unified robot controller with:
|
||||
* - Feetech servo control (SCS/STS)
|
||||
* - Animation playback via node graphs
|
||||
* - ADXL345 IMU
|
||||
* - RD-03D mmWave radar
|
||||
* - CRC16 tagged packet protocol
|
||||
*
|
||||
* Protocol: 0xA5 0x5A [TAG 4B][LEN 2B][SEQ 2B][PAYLOAD][CRC16 2B]
|
||||
*/
|
||||
|
||||
// Configuration defines
|
||||
#define ENABLE_SERIAL_PASSTHROUGH \
|
||||
false // Set true to use Feetech app comms straight to servos
|
||||
|
||||
#include "robotconfig.h"
|
||||
#include "animation.h"
|
||||
#include "commands.h"
|
||||
#include "motorcontrol.h"
|
||||
#include "motoraid.h"
|
||||
#include "nodegraph.h"
|
||||
#include "protocol.h"
|
||||
#include "sensors.h"
|
||||
#include "behaviors.h"
|
||||
#include <FFat.h>
|
||||
|
||||
|
||||
// ============================================================================
|
||||
// Global State
|
||||
// ============================================================================
|
||||
|
||||
RobotConfig config;
|
||||
|
||||
// Timing constants
|
||||
constexpr uint16_t FRAME_RATE = 48;
|
||||
constexpr uint16_t FRAME_INTERVAL_MS = 1000 / FRAME_RATE;
|
||||
constexpr uint16_t MOTOR_UPDATE_INTERVAL_MS = 50;
|
||||
constexpr uint32_t HEARTBEAT_INTERVAL_MS = 1000;
|
||||
|
||||
// ============================================================================
|
||||
// Utility Functions
|
||||
// ============================================================================
|
||||
|
||||
uint16_t getSineWaveValue(unsigned long centiseconds) {
|
||||
constexpr uint16_t WAVE_PERIOD_CS = 400; // 4 seconds
|
||||
constexpr uint16_t WAVE_MAX = 4095;
|
||||
|
||||
float theta = (2.0 * PI * centiseconds) / WAVE_PERIOD_CS;
|
||||
float sine = sin(theta);
|
||||
float scaled = (sine + 1.0) * (WAVE_MAX / 2.0);
|
||||
return (uint16_t)round(scaled);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Unified Motor Command Application
|
||||
// ============================================================================
|
||||
|
||||
// Apply motor commands from behaviors and/or animations
|
||||
// This function works whether an animation is active or not
|
||||
// Behaviors have priority - they override animation positions
|
||||
void applyMotorCommands(const std::vector<uint8_t>& motorIDs,
|
||||
const std::vector<uint16_t>& positions) {
|
||||
if (motorIDs.empty()) return;
|
||||
|
||||
std::vector<uint8_t> finalMotorIDs;
|
||||
std::vector<uint16_t> finalPositions;
|
||||
std::vector<uint16_t> speeds;
|
||||
|
||||
for (size_t i = 0; i < motorIDs.size(); i++) {
|
||||
uint8_t motorID = motorIDs[i];
|
||||
uint16_t finalPosition = positions[i];
|
||||
|
||||
// Check if a behavior wants to control this motor (behaviors have priority)
|
||||
uint16_t behaviorPosition;
|
||||
if (behaviorManager.getMotorPosition(motorID, behaviorPosition)) {
|
||||
// Behavior is controlling this motor, use its position instead
|
||||
finalPosition = behaviorPosition;
|
||||
}
|
||||
|
||||
finalMotorIDs.push_back(motorID);
|
||||
finalPositions.push_back(finalPosition);
|
||||
speeds.push_back(0);
|
||||
config.setMotorPosition(motorID, finalPosition);
|
||||
config.setMotorEnabled(motorID, true);
|
||||
}
|
||||
|
||||
// Send all positions in one sync write
|
||||
if (!finalMotorIDs.empty()) {
|
||||
servoManager.syncWritePositions(finalMotorIDs.data(), finalPositions.data(),
|
||||
speeds.data(), finalMotorIDs.size(), config, 0);
|
||||
}
|
||||
}
|
||||
|
||||
// Apply motor commands from behaviors only (when no animation is active)
|
||||
void applyBehaviorMotorCommands() {
|
||||
// Get all motors that behaviors want to control
|
||||
std::vector<uint8_t> motorIDs;
|
||||
std::vector<uint16_t> positions;
|
||||
|
||||
// Check all configured motors to see if any behavior wants to control them
|
||||
for (const Motor& motor : config.motors) {
|
||||
uint16_t position;
|
||||
if (behaviorManager.getMotorPosition(motor.motorID, position)) {
|
||||
motorIDs.push_back(motor.motorID);
|
||||
positions.push_back(position);
|
||||
}
|
||||
}
|
||||
|
||||
// Apply the behavior-controlled motors
|
||||
if (!motorIDs.empty()) {
|
||||
applyMotorCommands(motorIDs, positions);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Protocol Handler
|
||||
// ============================================================================
|
||||
|
||||
void handleProtocol() {
|
||||
if (receivePacket()) {
|
||||
dispatchCommand();
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Serial Passthrough (USB ↔ Servo Serial)
|
||||
// ============================================================================
|
||||
|
||||
#if ENABLE_SERIAL_PASSTHROUGH
|
||||
void handleSerialPassthrough() {
|
||||
// Forward USB Serial → Servo Serial1
|
||||
while (Serial.available()) {
|
||||
Serial1.write(Serial.read());
|
||||
}
|
||||
|
||||
// Forward Servo Serial1 → USB Serial
|
||||
while (Serial1.available()) {
|
||||
Serial.write(Serial1.read());
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
// ============================================================================
|
||||
// Animation Playback
|
||||
// ============================================================================
|
||||
|
||||
// Dispatcher: calls the appropriate animation function based on version
|
||||
void runAnimation() {
|
||||
if (!animState.current || !animState.current->isActive()) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (animState.current->header.version == 2) {
|
||||
runFrameAnimation();
|
||||
} else {
|
||||
runNodeAnimation();
|
||||
}
|
||||
}
|
||||
|
||||
// Version 2: Frame-by-frame animation playback
|
||||
void runFrameAnimation() {
|
||||
static uint32_t lastTickTime = 0;
|
||||
static uint32_t currentTick = 0;
|
||||
static uint8_t lastGeneration = 0;
|
||||
|
||||
if (!animState.current || !animState.current->isActive()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Reset tick when a new animation starts (detected by generation change)
|
||||
if (lastGeneration != animState.playGeneration) {
|
||||
currentTick = animState.startFrame;
|
||||
lastTickTime = millis();
|
||||
lastGeneration = animState.playGeneration;
|
||||
sendMessage("V2 Animation started, generation: " + String(lastGeneration) + ", startFrame: " + String(animState.startFrame));
|
||||
}
|
||||
|
||||
config.enableAllMotors();
|
||||
|
||||
// Calculate frame interval from animation's frame rate
|
||||
uint16_t frameIntervalMs = 1000 / animState.current->header.frameRate;
|
||||
if (frameIntervalMs == 0) frameIntervalMs = 1; // Safety: prevent division by zero
|
||||
|
||||
uint32_t now = millis();
|
||||
if (now - lastTickTime < frameIntervalMs)
|
||||
return;
|
||||
lastTickTime = now;
|
||||
|
||||
// Get frame data for current tick
|
||||
const std::vector<MotorPosition>* frameData = animState.current->getFrameData(currentTick);
|
||||
|
||||
if (frameData && !frameData->empty()) {
|
||||
// Collect motor commands from frame data
|
||||
std::vector<uint8_t> motorIDs;
|
||||
std::vector<uint16_t> positions;
|
||||
|
||||
for (const auto& motorPos : *frameData) {
|
||||
motorIDs.push_back(motorPos.motorID);
|
||||
positions.push_back(motorPos.position);
|
||||
}
|
||||
|
||||
// Apply motor commands (behaviors will override if needed)
|
||||
applyMotorCommands(motorIDs, positions);
|
||||
|
||||
// Debug: print frame and motor positions
|
||||
// Serial.print("Frame ");
|
||||
// Serial.print(currentTick);
|
||||
// Serial.print(": ");
|
||||
// for (size_t i = 0; i < motorIDs.size(); i++) {
|
||||
// if (i > 0) Serial.print(", ");
|
||||
// Serial.print("M");
|
||||
// Serial.print(motorIDs[i]);
|
||||
// Serial.print("=");
|
||||
// Serial.print(positions[i]);
|
||||
// }
|
||||
// Serial.println();
|
||||
}
|
||||
|
||||
// Emit per-frame event
|
||||
{
|
||||
uint8_t payload[4];
|
||||
payload[0] = currentTick & 0xFF;
|
||||
payload[1] = (currentTick >> 8) & 0xFF;
|
||||
payload[2] = static_cast<uint8_t>(animState.playMode);
|
||||
payload[3] = 0; // in-progress
|
||||
sendPacket(Tag::FRAME, payload, 4);
|
||||
}
|
||||
|
||||
currentTick++;
|
||||
|
||||
// Handle animation end
|
||||
uint16_t totalFrames = animState.current->getFrameCount();
|
||||
uint16_t framesPlayed = currentTick - animState.startFrame;
|
||||
uint16_t remainingFrames = (totalFrames > animState.startFrame) ? (totalFrames - animState.startFrame) : 0;
|
||||
|
||||
// Debug: log completion check near the end
|
||||
if (remainingFrames > 0 && framesPlayed >= remainingFrames - 1) {
|
||||
sendMessage("Completion check - currentTick: " + String(currentTick) + ", framesPlayed: " + String(framesPlayed) + ", remainingFrames: " + String(remainingFrames) + ", totalFrames: " + String(totalFrames));
|
||||
}
|
||||
|
||||
// Check if we've played all remaining frames (from startFrame to totalFrames-1)
|
||||
if (totalFrames > 0 && remainingFrames > 0 && framesPlayed >= remainingFrames) {
|
||||
sendMessage("Animation completion triggered! Sending completion packet.");
|
||||
switch (animState.playMode) {
|
||||
case PLAY_ONCE:
|
||||
{
|
||||
// Send completion packet BEFORE stopping animation
|
||||
uint8_t done[4];
|
||||
done[0] = currentTick & 0xFF;
|
||||
done[1] = (currentTick >> 8) & 0xFF;
|
||||
done[2] = static_cast<uint8_t>(animState.playMode);
|
||||
done[3] = 1; // complete
|
||||
sendPacket(Tag::FRAME, done, 4);
|
||||
animState.stop();
|
||||
}
|
||||
break;
|
||||
case PLAY_LOOP:
|
||||
// Reset to start frame for seamless looping
|
||||
currentTick = animState.startFrame;
|
||||
break;
|
||||
case PLAY_REPEAT:
|
||||
if (--animState.repeatsRemaining == 0) {
|
||||
// Send completion packet BEFORE stopping animation
|
||||
uint8_t done[4];
|
||||
done[0] = currentTick & 0xFF;
|
||||
done[1] = (currentTick >> 8) & 0xFF;
|
||||
done[2] = static_cast<uint8_t>(animState.playMode);
|
||||
done[3] = 1; // complete
|
||||
sendPacket(Tag::FRAME, done, 4);
|
||||
animState.stop();
|
||||
} else {
|
||||
currentTick = animState.startFrame;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Version 1: Node graph animation playback
|
||||
void runNodeAnimation() {
|
||||
static uint32_t lastTickTime = 0;
|
||||
static uint32_t currentTick = 0;
|
||||
static uint8_t lastGeneration = 0;
|
||||
|
||||
if (!animState.current || !animState.current->isActive()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Reset tick when a new animation starts (detected by generation change)
|
||||
if (lastGeneration != animState.playGeneration) {
|
||||
currentTick = animState.startFrame; // Start from specified frame
|
||||
lastTickTime = millis();
|
||||
lastGeneration = animState.playGeneration;
|
||||
// Debug: send startFrame via MSGE
|
||||
sendMessage("Animation startFrame: " + String(animState.startFrame) + ", currentTick: " + String(currentTick));
|
||||
}
|
||||
|
||||
config.enableAllMotors();
|
||||
|
||||
// Calculate frame interval from animation's frame rate
|
||||
uint16_t frameIntervalMs = 1000 / animState.current->header.frameRate;
|
||||
if (frameIntervalMs == 0) frameIntervalMs = 1; // Safety: prevent division by zero
|
||||
|
||||
uint32_t now = millis();
|
||||
if (now - lastTickTime < frameIntervalMs)
|
||||
return;
|
||||
lastTickTime = now;
|
||||
|
||||
// Tick the node graph
|
||||
animState.current->nodeGraph.tick(currentTick, *animState.current);
|
||||
auto outputs = animState.current->nodeGraph.getServoOutputs();
|
||||
|
||||
// Collect motor commands
|
||||
std::vector<uint8_t> motorIDs;
|
||||
std::vector<uint16_t> positions;
|
||||
|
||||
for (const auto &[motorID, value] : outputs) {
|
||||
if (value != 65535) {
|
||||
motorIDs.push_back(motorID);
|
||||
positions.push_back(value);
|
||||
} else {
|
||||
// Only disable torque for motors that should be limp
|
||||
if (config.setMotorEnabled(motorID, false)) {
|
||||
servoManager[0]->disableTorque(motorID);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Apply motor commands (behaviors will override if needed)
|
||||
applyMotorCommands(motorIDs, positions);
|
||||
|
||||
// Emit per-frame event: [frameLo, frameHi, playMode, status=0]
|
||||
// Send actual frame number (currentTick), not relative frame
|
||||
{
|
||||
uint8_t payload[4];
|
||||
payload[0] = currentTick & 0xFF;
|
||||
payload[1] = (currentTick >> 8) & 0xFF;
|
||||
payload[2] = static_cast<uint8_t>(animState.playMode);
|
||||
payload[3] = 0; // in-progress
|
||||
sendPacket(Tag::FRAME, payload, 4);
|
||||
}
|
||||
|
||||
currentTick++;
|
||||
|
||||
// Handle animation end (0 = run indefinitely for variable-only animations)
|
||||
// Calculate total frames played: currentTick - startFrame
|
||||
uint16_t framesPlayed = currentTick - animState.startFrame;
|
||||
// Calculate remaining frames: total frames minus startFrame
|
||||
// If animation has 100 frames and we start at 50, we should only play 50 frames
|
||||
uint16_t totalFrames = animState.current->getFrameCount();
|
||||
uint16_t remainingFrames = (totalFrames > animState.startFrame) ? (totalFrames - animState.startFrame) : 0;
|
||||
|
||||
// Debug: show completion check values every 10 frames
|
||||
if (framesPlayed % 10 == 0 || framesPlayed >= remainingFrames - 1) {
|
||||
sendMessage("Frame check - played: " + String(framesPlayed) + ", remaining: " + String(remainingFrames) + ", total: " + String(totalFrames) + ", startFrame: " + String(animState.startFrame));
|
||||
}
|
||||
|
||||
if (totalFrames > 0 && remainingFrames > 0 && framesPlayed >= remainingFrames) {
|
||||
switch (animState.playMode) {
|
||||
case PLAY_ONCE:
|
||||
animState.stop();
|
||||
{
|
||||
uint8_t done[4];
|
||||
done[0] = currentTick & 0xFF;
|
||||
done[1] = (currentTick >> 8) & 0xFF;
|
||||
done[2] = static_cast<uint8_t>(animState.playMode);
|
||||
done[3] = 1; // complete
|
||||
sendPacket(Tag::FRAME, done, 4);
|
||||
}
|
||||
break;
|
||||
case PLAY_LOOP:
|
||||
// Reset to start frame for seamless looping
|
||||
currentTick = animState.startFrame;
|
||||
sendMessage("Looping back to startFrame: " + String(animState.startFrame));
|
||||
break;
|
||||
case PLAY_REPEAT:
|
||||
if (--animState.repeatsRemaining == 0) {
|
||||
animState.stop();
|
||||
uint8_t done[4];
|
||||
done[0] = currentTick & 0xFF;
|
||||
done[1] = (currentTick >> 8) & 0xFF;
|
||||
done[2] = static_cast<uint8_t>(animState.playMode);
|
||||
done[3] = 1; // complete
|
||||
sendPacket(Tag::FRAME, done, 4);
|
||||
// Animation stopped, don't reset tick
|
||||
} else {
|
||||
// Reset to start frame for next repeat
|
||||
currentTick = animState.startFrame;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
// Don't reset currentTick here - each case handles it if needed
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Test Functions
|
||||
// ============================================================================
|
||||
|
||||
void testSweepMotor40() {
|
||||
static unsigned long lastUpdate = 0;
|
||||
static uint16_t position = 500;
|
||||
static int16_t direction = 1;
|
||||
const unsigned long SWEEP_INTERVAL_MS = 20; // Update every 20ms
|
||||
const uint16_t MIN_POS = 500;
|
||||
const uint16_t MAX_POS = 2500;
|
||||
const uint16_t STEP = 10;
|
||||
|
||||
unsigned long now = millis();
|
||||
if (now - lastUpdate < SWEEP_INTERVAL_MS) {
|
||||
return;
|
||||
}
|
||||
lastUpdate = now;
|
||||
|
||||
// Update position
|
||||
position += (direction * STEP);
|
||||
|
||||
// Reverse direction at limits
|
||||
if (position >= MAX_POS) {
|
||||
position = MAX_POS;
|
||||
direction = -1;
|
||||
} else if (position <= MIN_POS) {
|
||||
position = MIN_POS;
|
||||
direction = 1;
|
||||
}
|
||||
|
||||
// Send position to motor 40
|
||||
uint8_t motorID = 40;
|
||||
uint16_t positions[1] = {position};
|
||||
uint16_t speeds[1] = {0};
|
||||
uint8_t ids[1] = {motorID};
|
||||
|
||||
servoManager.syncWritePositions(ids, positions, speeds, 1, config, 0);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Motor Position Updates
|
||||
// ============================================================================
|
||||
|
||||
void updateMotorPositions() {
|
||||
// Only read positions when motor streaming is active
|
||||
// This prevents blocking the main loop when positions aren't needed
|
||||
if (!motorStream.active) {
|
||||
return;
|
||||
}
|
||||
|
||||
static unsigned long lastUpdate = 0;
|
||||
|
||||
if (millis() - lastUpdate < MOTOR_UPDATE_INTERVAL_MS)
|
||||
return;
|
||||
lastUpdate = millis();
|
||||
|
||||
for (const Motor &motor : config.motors) {
|
||||
servoManager[0]->setFeetechMode(motor.servoModel.major);
|
||||
uint16_t position = servoManager[0]->getPosition(motor.motorID);
|
||||
config.setMotorPosition(motor.motorID, position);
|
||||
uint16_t current = servoManager[0]->getCurrent(motor.motorID);
|
||||
config.setMotorCurrent(motor.motorID, current);
|
||||
}
|
||||
}
|
||||
|
||||
void printMotorStats() {
|
||||
Serial.println("--- Motor stats ---");
|
||||
for (const Motor& motor : config.motors) {
|
||||
Serial.print("ID:");
|
||||
Serial.print(motor.motorID);
|
||||
Serial.print(" pos:");
|
||||
Serial.print(motor.position);
|
||||
Serial.print(" cur:");
|
||||
Serial.println(motor.current);
|
||||
}
|
||||
Serial.println("-------------------");
|
||||
}
|
||||
|
||||
void handleMotorStreaming() {
|
||||
if (motorStream.shouldStream()) {
|
||||
sendMotorPositions();
|
||||
}
|
||||
}
|
||||
|
||||
void printLoopRate() {
|
||||
static unsigned long lastPrint = 0;
|
||||
static uint32_t loopCount = 0;
|
||||
|
||||
loopCount++;
|
||||
|
||||
unsigned long now = millis();
|
||||
if (now - lastPrint >= 1000) {
|
||||
Serial.print("[Loop] ");
|
||||
Serial.print(loopCount);
|
||||
Serial.println(" Hz");
|
||||
loopCount = 0;
|
||||
lastPrint = now;
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Heartbeat
|
||||
// ============================================================================
|
||||
|
||||
void sendHeartbeat() {
|
||||
static unsigned long lastHeartbeat = 0;
|
||||
|
||||
if (millis() - lastHeartbeat < HEARTBEAT_INTERVAL_MS)
|
||||
return;
|
||||
lastHeartbeat = millis();
|
||||
|
||||
// Pack state: uptime(4) + flags(2)
|
||||
uint8_t payload[6];
|
||||
uint32_t uptime = millis() / 1000;
|
||||
uint16_t flags = 0;
|
||||
|
||||
// Build flags
|
||||
if (adxl.isReady())
|
||||
flags |= 0x01;
|
||||
if (animState.current && animState.current->isActive())
|
||||
flags |= 0x02;
|
||||
if (motorStream.active)
|
||||
flags |= 0x04;
|
||||
if (sensors.isADXLStreamEnabled())
|
||||
flags |= 0x08;
|
||||
if (sensors.isRadarStreamEnabled())
|
||||
flags |= 0x10;
|
||||
|
||||
payload[0] = uptime & 0xFF;
|
||||
payload[1] = (uptime >> 8) & 0xFF;
|
||||
payload[2] = (uptime >> 16) & 0xFF;
|
||||
payload[3] = (uptime >> 24) & 0xFF;
|
||||
payload[4] = flags & 0xFF;
|
||||
payload[5] = (flags >> 8) & 0xFF;
|
||||
|
||||
sendPacket(Tag::STATE, payload, 6);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Setup
|
||||
// ============================================================================
|
||||
|
||||
void setup() {
|
||||
// Serial setup (buffer size must be set before begin)
|
||||
Serial.setRxBufferSize(8192);
|
||||
Serial.begin(1000000);
|
||||
|
||||
pinMode(6, OUTPUT);
|
||||
digitalWrite(6, 1);
|
||||
|
||||
// Startup delay
|
||||
delay(500);
|
||||
Serial.println("\n[HansonServo] Starting...");
|
||||
|
||||
// Initialize servo manager
|
||||
servoManager.init();
|
||||
Serial.println("[HansonServo] Servos initialized");
|
||||
|
||||
// Initialize sensors
|
||||
sensors.init();
|
||||
|
||||
// Initialize filesystem
|
||||
if (!FFat.begin(true)) {
|
||||
Serial.println("[HansonServo] FFat mount failed!");
|
||||
return;
|
||||
}
|
||||
Serial.println("[HansonServo] Filesystem ready");
|
||||
|
||||
|
||||
|
||||
// Initialize behaviors (order determines priority: first added = highest priority)
|
||||
// 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;
|
||||
behaviorManager.addBehavior(BEHAVIOR_FOCUS, &focusBehavior);
|
||||
|
||||
// 2. Viseme behavior (medium priority - mouth animation for speech)
|
||||
behaviorManager.addBehavior(BEHAVIOR_VISEME, &visemeBehavior);
|
||||
|
||||
// 3. Idle behavior (lowest priority - perlin noise for all motors)
|
||||
static IdleBehavior idleBehavior;
|
||||
behaviorManager.addBehavior(BEHAVIOR_IDLE, &idleBehavior);
|
||||
|
||||
Serial.println("[HansonServo] Behaviors initialized (focus > viseme > idle)");
|
||||
|
||||
// Check if config file exists before loading
|
||||
bool configExisted = FFat.exists("/robot_config.bin");
|
||||
|
||||
// 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);
|
||||
|
||||
if (configLoaded) {
|
||||
Serial.println("[HansonServo] Config loaded: " + config.deviceName);
|
||||
|
||||
// If config file existed before, behavior states should have been loaded
|
||||
// If it's a new file, we need to set defaults
|
||||
if (!configExisted) {
|
||||
Serial.println("[HansonServo] New config file, setting default behavior states...");
|
||||
behaviorManager.setBehaviorEnabled(BEHAVIOR_FOCUS, true);
|
||||
behaviorManager.setBehaviorEnabled(BEHAVIOR_VISEME, true);
|
||||
behaviorManager.setBehaviorEnabled(BEHAVIOR_IDLE, true);
|
||||
// Save the defaults
|
||||
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
|
||||
} else {
|
||||
Serial.println("[HansonServo] Behavior states loaded from config");
|
||||
}
|
||||
|
||||
// Check if visemes were loaded from config
|
||||
if (visemeBehavior.getVisemes().empty()) {
|
||||
Serial.println("[HansonServo] No visemes in config, creating defaults...");
|
||||
// Only create defaults if config had no visemes
|
||||
visemeBehavior.addViseme(0, "SIL", 2047, 2047, 2047); // Neutral/rest (sil)
|
||||
visemeBehavior.addViseme(1, "AA ", 2200, 1900, 2100); // AA (as in "father")
|
||||
visemeBehavior.addViseme(2, "AE ", 2100, 2000, 2000); // AE (as in "cat")
|
||||
visemeBehavior.addViseme(3, "AH ", 2150, 1950, 2050); // AH (as in "but")
|
||||
visemeBehavior.addViseme(4, "AO ", 2000, 2100, 1950); // AO (as in "bought")
|
||||
visemeBehavior.addViseme(5, "EH ", 1900, 2200, 1900); // EH (as in "bed")
|
||||
visemeBehavior.addViseme(6, "IH ", 1850, 2250, 1850); // IH (as in "bit")
|
||||
visemeBehavior.addViseme(7, "IY ", 1800, 2300, 1800); // IY (as in "beat")
|
||||
visemeBehavior.addViseme(8, "OW ", 2300, 1800, 2200); // OW (as in "boat")
|
||||
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);
|
||||
} else {
|
||||
Serial.println("[HansonServo] Visemes loaded from config: " + String(visemeBehavior.getVisemes().size()));
|
||||
}
|
||||
} else {
|
||||
Serial.println("[HansonServo] Config init failed");
|
||||
// Set defaults on failure
|
||||
behaviorManager.setBehaviorEnabled(BEHAVIOR_FOCUS, true);
|
||||
behaviorManager.setBehaviorEnabled(BEHAVIOR_VISEME, true);
|
||||
behaviorManager.setBehaviorEnabled(BEHAVIOR_IDLE, true);
|
||||
}
|
||||
|
||||
// Initialize idle behavior motors (needs config.motors to be loaded)
|
||||
std::vector<uint8_t> allMotorIDs;
|
||||
for (const Motor& motor : config.motors) {
|
||||
allMotorIDs.push_back(motor.motorID);
|
||||
}
|
||||
idleBehavior.initMotors(allMotorIDs);
|
||||
|
||||
motorStream.start();
|
||||
|
||||
// Initialize motor-aided movement
|
||||
// Motor 25 - default settings
|
||||
motorAid.addMotor(25);
|
||||
|
||||
// Motors 30, 31, 35, 36 - high gear ratio, maximum sensitivity
|
||||
AidedMotorSettings highGearSettings;
|
||||
highGearSettings.velocityThreshold = 8; // Extremely sensitive trigger
|
||||
highGearSettings.resetThreshold = 4; // Minimal hysteresis
|
||||
highGearSettings.leadOffset = 100; // Smaller lead for fine control
|
||||
highGearSettings.assistSpeed = 600; // Moderate speed
|
||||
highGearSettings.assistDuration = 400; // Shorter burst
|
||||
|
||||
motorAid.addMotor(30, highGearSettings);
|
||||
motorAid.addMotor(31, highGearSettings);
|
||||
motorAid.addMotor(35, highGearSettings);
|
||||
motorAid.addMotor(36, highGearSettings);
|
||||
|
||||
Serial.println("[HansonServo] Ready");
|
||||
Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16");
|
||||
|
||||
// ---- TEST: Load and play animation ----
|
||||
// Serial.println("[TEST] Loading /slow.anim...");
|
||||
// if (animState.animation.loadFromFile("/slow.anim")) {
|
||||
// Serial.println("[TEST] Animation loaded successfully");
|
||||
|
||||
// delay(1000); // Wait 1 second
|
||||
|
||||
// // Print animation info
|
||||
// Serial.println(animState.animation.printAnim());
|
||||
|
||||
// delay(5000); // Wait 5 seconds
|
||||
|
||||
// // Play the animation
|
||||
// Serial.println("[TEST] Playing animation...");
|
||||
// animState.play(PLAY_ONCE, 1, 0);
|
||||
// } else {
|
||||
// Serial.println("[TEST] Failed to load /animation.anim");
|
||||
// }
|
||||
// ---- END TEST ----
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Main Loop
|
||||
// ============================================================================
|
||||
|
||||
void loop() {
|
||||
// Serial passthrough (when enabled)
|
||||
#if ENABLE_SERIAL_PASSTHROUGH
|
||||
handleSerialPassthrough();
|
||||
return;
|
||||
#endif
|
||||
|
||||
// Protocol handling
|
||||
handleProtocol();
|
||||
|
||||
// Update behaviors
|
||||
behaviorManager.update();
|
||||
|
||||
// Animation playback (auto-selects v1 node or v2 frame based on version)
|
||||
runAnimation();
|
||||
|
||||
// If no animation is active, still apply behavior motor commands
|
||||
if (!animState.current || !animState.current->isActive()) {
|
||||
applyBehaviorMotorCommands();
|
||||
}
|
||||
|
||||
// Motor position updates
|
||||
updateMotorPositions();
|
||||
handleMotorStreaming();
|
||||
|
||||
// Motor-aided movement (when motorStream is active)
|
||||
if (motorStream.active) {
|
||||
motorAid.update();
|
||||
}
|
||||
//printMotorStats();
|
||||
// Sensor updates and streaming
|
||||
//sensors.update();
|
||||
|
||||
// Heartbeat
|
||||
//sendHeartbeat();
|
||||
|
||||
// Debug: print loop rate
|
||||
printLoopRate();
|
||||
|
||||
// ============================================================================
|
||||
// TEST: Uncomment to enable motor 40 sweep test
|
||||
// ============================================================================
|
||||
//testSweepMotor40();
|
||||
}
|
||||
221
motoraid.cpp
221
motoraid.cpp
|
|
@ -1,221 +0,0 @@
|
|||
#include "motoraid.h"
|
||||
#include "motorcontrol.h"
|
||||
#include "robotconfig.h"
|
||||
|
||||
// Global instance
|
||||
MotorAid motorAid;
|
||||
|
||||
extern RobotConfig config;
|
||||
|
||||
void MotorAid::addMotor(uint8_t motorID) {
|
||||
// Use default settings
|
||||
AidedMotorSettings defaultSettings;
|
||||
addMotor(motorID, defaultSettings);
|
||||
}
|
||||
|
||||
void MotorAid::addMotor(uint8_t motorID, const AidedMotorSettings& settings) {
|
||||
// Don't add duplicates
|
||||
if (findMotor(motorID) != nullptr) return;
|
||||
|
||||
AidedMotor motor;
|
||||
motor.motorID = motorID;
|
||||
motor.settings = settings;
|
||||
|
||||
// Read actual position directly from servo (not config which may be stale)
|
||||
uint8_t model = config.getMotorModel(motorID);
|
||||
servoManager[0]->setFeetechMode(model);
|
||||
motor.lastPosition = servoManager[0]->getPosition(motorID);
|
||||
motor.stablePosition = motor.lastPosition; // Initialize stable position
|
||||
motor.lastUpdateTime = millis();
|
||||
|
||||
// Initialize velocity samples to zero
|
||||
for (uint8_t i = 0; i < VELOCITY_SAMPLES; i++) {
|
||||
motor.velocitySamples[i] = 0;
|
||||
}
|
||||
|
||||
aidedMotors.push_back(motor);
|
||||
|
||||
// Disable torque on this motor so it can be moved freely
|
||||
servoManager[0]->disableTorque(motorID);
|
||||
|
||||
Serial.print("[MotorAid] Added motor ");
|
||||
Serial.print(motorID);
|
||||
Serial.print(" at pos ");
|
||||
Serial.print(motor.lastPosition);
|
||||
Serial.print(" (thresh=");
|
||||
Serial.print(settings.velocityThreshold);
|
||||
Serial.print(", lead=");
|
||||
Serial.print(settings.leadOffset);
|
||||
Serial.println(")");
|
||||
}
|
||||
|
||||
void MotorAid::removeMotor(uint8_t motorID) {
|
||||
for (auto it = aidedMotors.begin(); it != aidedMotors.end(); ++it) {
|
||||
if (it->motorID == motorID) {
|
||||
aidedMotors.erase(it);
|
||||
Serial.print("[MotorAid] Removed motor ");
|
||||
Serial.println(motorID);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool MotorAid::isMotorAided(uint8_t motorID) const {
|
||||
return findMotor(motorID) != nullptr;
|
||||
}
|
||||
|
||||
AidedMotorSettings* MotorAid::getMotorSettings(uint8_t motorID) {
|
||||
AidedMotor* motor = findMotor(motorID);
|
||||
if (motor) return &motor->settings;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
AidedMotor* MotorAid::findMotor(uint8_t motorID) {
|
||||
for (auto& motor : aidedMotors) {
|
||||
if (motor.motorID == motorID) return &motor;
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
const AidedMotor* MotorAid::findMotor(uint8_t motorID) const {
|
||||
for (const auto& motor : aidedMotors) {
|
||||
if (motor.motorID == motorID) return &motor;
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
int16_t MotorAid::calculateSmoothedVelocity(AidedMotor& motor) {
|
||||
// Calculate average of velocity samples
|
||||
int32_t sum = 0;
|
||||
for (uint8_t i = 0; i < VELOCITY_SAMPLES; i++) {
|
||||
sum += motor.velocitySamples[i];
|
||||
}
|
||||
return (int16_t)(sum / VELOCITY_SAMPLES);
|
||||
}
|
||||
|
||||
void MotorAid::update() {
|
||||
unsigned long now = millis();
|
||||
|
||||
for (AidedMotor& motor : aidedMotors) {
|
||||
const AidedMotorSettings& s = motor.settings; // Shorthand
|
||||
|
||||
// Calculate time delta
|
||||
unsigned long deltaTime = now - motor.lastUpdateTime;
|
||||
if (deltaTime < updateInterval) continue;
|
||||
|
||||
// Get current position from config (already updated by updateMotorPositions)
|
||||
uint16_t currentPosition = config.getMotorPosition(motor.motorID);
|
||||
|
||||
// Calculate position delta from stable position (filters noise)
|
||||
int16_t positionDelta = (int16_t)currentPosition - (int16_t)motor.stablePosition;
|
||||
|
||||
// Only register movement if it exceeds minimum delta (noise filter)
|
||||
int16_t instantVelocity = 0;
|
||||
if (abs(positionDelta) >= MIN_POSITION_DELTA) {
|
||||
// Significant movement - calculate velocity and update stable position
|
||||
instantVelocity = (positionDelta * 1000) / (int16_t)deltaTime;
|
||||
motor.stablePosition = currentPosition;
|
||||
}
|
||||
// else: tiny movement, treat as zero velocity (noise)
|
||||
|
||||
// Add to velocity samples (circular buffer)
|
||||
motor.velocitySamples[motor.sampleIndex] = instantVelocity;
|
||||
motor.sampleIndex = (motor.sampleIndex + 1) % VELOCITY_SAMPLES;
|
||||
|
||||
// Track samples collected for warmup
|
||||
if (motor.samplesCollected < VELOCITY_SAMPLES * 2) {
|
||||
motor.samplesCollected++;
|
||||
}
|
||||
|
||||
// Don't calculate or trigger until we have enough samples (2x buffer = ~300ms warmup)
|
||||
if (motor.samplesCollected < VELOCITY_SAMPLES * 2) {
|
||||
continue; // Still warming up
|
||||
}
|
||||
motor.warmedUp = true;
|
||||
|
||||
// Calculate smoothed velocity
|
||||
motor.smoothedVelocity = calculateSmoothedVelocity(motor);
|
||||
|
||||
// Update tracking
|
||||
motor.lastUpdateTime = now;
|
||||
|
||||
// Check if currently assisting
|
||||
if (motor.assisting) {
|
||||
// Calculate target position AHEAD of current position in direction of movement
|
||||
int16_t leadOffset = (motor.smoothedVelocity > 0) ? s.leadOffset : -s.leadOffset;
|
||||
motor.assistTargetPosition = constrain((int16_t)currentPosition + leadOffset, 0, 4095);
|
||||
|
||||
// Send updated position command to lead the movement
|
||||
uint8_t model = config.getMotorModel(motor.motorID);
|
||||
servoManager[0]->setFeetechMode(model);
|
||||
|
||||
uint8_t ids[1] = { motor.motorID };
|
||||
uint16_t positions[1] = { motor.assistTargetPosition };
|
||||
uint16_t speeds[1] = { s.assistSpeed };
|
||||
servoManager.syncWritePositions(ids, positions, speeds, 1, config, 0);
|
||||
|
||||
// Check if assist duration has elapsed (fixed duration, always ends)
|
||||
if (now - motor.assistStartTime >= s.assistDuration) {
|
||||
motor.assisting = false;
|
||||
// Disable torque again
|
||||
servoManager[0]->disableTorque(motor.motorID);
|
||||
|
||||
Serial.print("[MotorAid] Assist ended, motor ");
|
||||
Serial.print(motor.motorID);
|
||||
Serial.print(" at pos ");
|
||||
Serial.println(currentPosition);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
// Hysteresis: if we were triggered, wait for velocity to drop before re-triggering
|
||||
if (motor.wasTriggered) {
|
||||
if (abs(motor.smoothedVelocity) < s.resetThreshold) {
|
||||
motor.wasTriggered = false; // Reset, can trigger again
|
||||
motor.consecutiveHighCount = 0; // Reset consecutive counter too
|
||||
}
|
||||
continue; // Don't trigger while in hysteresis zone
|
||||
}
|
||||
|
||||
// Check if smoothed velocity exceeds threshold (in either direction)
|
||||
if (abs(motor.smoothedVelocity) > s.velocityThreshold) {
|
||||
motor.consecutiveHighCount++;
|
||||
} else {
|
||||
motor.consecutiveHighCount = 0; // Reset on low reading
|
||||
}
|
||||
|
||||
// Only trigger after CONSECUTIVE_REQUIRED high readings (filters noise spikes)
|
||||
if (motor.consecutiveHighCount >= CONSECUTIVE_REQUIRED) {
|
||||
// Sustained movement detected - assist!
|
||||
motor.assisting = true;
|
||||
motor.assistStartTime = now;
|
||||
motor.wasTriggered = true;
|
||||
|
||||
// Calculate target position AHEAD of current position in direction of movement
|
||||
int16_t leadOffset = (motor.smoothedVelocity > 0) ? s.leadOffset : -s.leadOffset;
|
||||
motor.assistTargetPosition = constrain((int16_t)currentPosition + leadOffset, 0, 4095);
|
||||
|
||||
// Set servo mode based on motor model
|
||||
uint8_t model = config.getMotorModel(motor.motorID);
|
||||
servoManager[0]->setFeetechMode(model);
|
||||
|
||||
// Enable torque and move AHEAD to assist
|
||||
servoManager[0]->enableTorque(motor.motorID);
|
||||
|
||||
uint8_t ids[1] = { motor.motorID };
|
||||
uint16_t positions[1] = { motor.assistTargetPosition };
|
||||
uint16_t speeds[1] = { s.assistSpeed };
|
||||
servoManager.syncWritePositions(ids, positions, speeds, 1, config, 0);
|
||||
|
||||
// Debug output
|
||||
Serial.print("[MotorAid] Assisting motor ");
|
||||
Serial.print(motor.motorID);
|
||||
Serial.print(" vel=");
|
||||
Serial.print(motor.smoothedVelocity);
|
||||
Serial.print(" pos=");
|
||||
Serial.print(currentPosition);
|
||||
Serial.print(" -> ");
|
||||
Serial.println(motor.assistTargetPosition);
|
||||
}
|
||||
}
|
||||
}
|
||||
82
motoraid.h
82
motoraid.h
|
|
@ -1,82 +0,0 @@
|
|||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include <vector>
|
||||
|
||||
// Motor-aided movement: detects external manipulation and assists to reduce gear stress
|
||||
// Works by monitoring position velocity - when someone moves the motor manually,
|
||||
// it briefly enables torque and moves to the current position to assist.
|
||||
|
||||
constexpr uint8_t VELOCITY_SAMPLES = 4; // Number of samples for velocity smoothing (fewer = faster response)
|
||||
constexpr int16_t MIN_POSITION_DELTA = 1; // Ignore position changes smaller than this (minimum for high gear ratio)
|
||||
constexpr uint8_t CONSECUTIVE_REQUIRED = 3; // Require this many consecutive high readings to trigger
|
||||
|
||||
struct AidedMotorSettings {
|
||||
int16_t velocityThreshold = 80; // Position units/sec to trigger assist (low for high gear ratio)
|
||||
int16_t resetThreshold = 30; // Velocity must drop below this to re-trigger
|
||||
int16_t leadOffset = 200; // How far ahead to command (position units)
|
||||
uint16_t assistSpeed = 1000; // Speed to use when assisting
|
||||
unsigned long assistDuration = 500; // ms to assist for
|
||||
};
|
||||
|
||||
struct AidedMotor {
|
||||
uint8_t motorID;
|
||||
uint16_t lastPosition = 0;
|
||||
unsigned long lastUpdateTime = 0;
|
||||
|
||||
// Per-motor settings
|
||||
AidedMotorSettings settings;
|
||||
|
||||
// Velocity smoothing
|
||||
int16_t velocitySamples[VELOCITY_SAMPLES] = {0};
|
||||
uint8_t sampleIndex = 0;
|
||||
uint8_t samplesCollected = 0; // Track how many samples we've collected
|
||||
int16_t smoothedVelocity = 0;
|
||||
uint16_t stablePosition = 0; // Position used for delta calculation (filters noise)
|
||||
|
||||
// Assist state
|
||||
bool assisting = false;
|
||||
unsigned long assistStartTime = 0;
|
||||
uint16_t assistTargetPosition = 0;
|
||||
|
||||
// Hysteresis - prevents re-triggering until velocity drops
|
||||
bool wasTriggered = false;
|
||||
|
||||
// Warmup - don't trigger until we have enough samples
|
||||
bool warmedUp = false;
|
||||
|
||||
// Consecutive high readings counter (filters noise spikes)
|
||||
uint8_t consecutiveHighCount = 0;
|
||||
};
|
||||
|
||||
class MotorAid {
|
||||
public:
|
||||
// Add a motor with default settings
|
||||
void addMotor(uint8_t motorID);
|
||||
|
||||
// Add a motor with custom settings
|
||||
void addMotor(uint8_t motorID, const AidedMotorSettings& settings);
|
||||
|
||||
// Remove a motor from aided list
|
||||
void removeMotor(uint8_t motorID);
|
||||
|
||||
// Check if a motor is being aided
|
||||
bool isMotorAided(uint8_t motorID) const;
|
||||
|
||||
// Get settings for a motor (to modify)
|
||||
AidedMotorSettings* getMotorSettings(uint8_t motorID);
|
||||
|
||||
// Main update function - call from loop() when motorStream is active
|
||||
void update();
|
||||
|
||||
private:
|
||||
std::vector<AidedMotor> aidedMotors;
|
||||
unsigned long updateInterval = 30; // ms between position checks
|
||||
|
||||
AidedMotor* findMotor(uint8_t motorID);
|
||||
const AidedMotor* findMotor(uint8_t motorID) const;
|
||||
|
||||
// Calculate smoothed velocity from samples
|
||||
int16_t calculateSmoothedVelocity(AidedMotor& motor);
|
||||
};
|
||||
|
||||
extern MotorAid motorAid;
|
||||
|
|
@ -1,60 +0,0 @@
|
|||
#include "motorcontrol.h"
|
||||
|
||||
// Global servo manager instance
|
||||
ServoManager servoManager;
|
||||
|
||||
uint16_t flipBytes(uint16_t value) {
|
||||
return (value >> 8) | (value << 8);
|
||||
}
|
||||
|
||||
void prepareMotorPositions(
|
||||
const uint8_t* ids,
|
||||
uint16_t* positions,
|
||||
uint16_t* speeds,
|
||||
uint8_t count,
|
||||
RobotConfig& config
|
||||
) {
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
if (config.getMotorModel(ids[i]) == MODEL_STS) {
|
||||
// STS servos: flip bytes, different speed range
|
||||
positions[i] = flipBytes(positions[i]);
|
||||
speeds[i] = map(speeds[i], 0, 4095, 0, 254);
|
||||
speeds[i] = flipBytes(speeds[i]);
|
||||
} else {
|
||||
// SCS servos: map to 10-bit range
|
||||
positions[i] = map(positions[i], 0, 4095, 0, 1023);
|
||||
speeds[i] = map(speeds[i], 0, 4095, 0, 1000);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ServoManager::init() {
|
||||
servos[0] = new Feetech(Serial1, Pins::DE, Pins::RE, Pins::CH0_TX, Pins::CH0_RX);
|
||||
servos[0]->setFeetechMode(Feetech::MODE_SCS);
|
||||
servos[0]->begin();
|
||||
|
||||
servos[1] = new Feetech(Serial2, Pins::DE, Pins::RE, Pins::CH1_TX, Pins::CH1_RX);
|
||||
servos[1]->setFeetechMode(Feetech::MODE_SCS);
|
||||
// servos[1]->begin(); // Uncomment when using channel 1
|
||||
}
|
||||
|
||||
void ServoManager::syncWritePositions(
|
||||
const uint8_t* ids,
|
||||
uint16_t* positions,
|
||||
uint16_t* speeds,
|
||||
uint8_t count,
|
||||
RobotConfig& config,
|
||||
uint8_t channel
|
||||
) {
|
||||
// Prepare positions (handles SCS vs STS conversion)
|
||||
prepareMotorPositions(ids, positions, speeds, count, config);
|
||||
|
||||
// Send to servos
|
||||
servos[channel]->syncWritePos(
|
||||
const_cast<uint8_t*>(ids),
|
||||
positions,
|
||||
speeds,
|
||||
count
|
||||
);
|
||||
}
|
||||
|
||||
|
|
@ -1,60 +0,0 @@
|
|||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include "feetech.h"
|
||||
#include "robotconfig.h"
|
||||
|
||||
// Pin definitions
|
||||
namespace Pins {
|
||||
// Channel 0 (SCS servos)
|
||||
constexpr int CH0_RX = 13;
|
||||
constexpr int CH0_TX = 12;
|
||||
|
||||
// Channel 1 (STS servos)
|
||||
constexpr int CH1_RX = 11;
|
||||
constexpr int CH1_TX = 10;
|
||||
|
||||
// RS485 control
|
||||
constexpr int DE = 7; // Driver Enable
|
||||
constexpr int RE = 8; // Receiver Enable
|
||||
}
|
||||
|
||||
// Servo model constants
|
||||
constexpr uint8_t MODEL_STS = 9;
|
||||
|
||||
// Utility functions
|
||||
uint16_t flipBytes(uint16_t value);
|
||||
|
||||
// Prepare motor positions for transmission (handles SCS vs STS byte ordering)
|
||||
void prepareMotorPositions(
|
||||
const uint8_t* ids,
|
||||
uint16_t* positions,
|
||||
uint16_t* speeds,
|
||||
uint8_t count,
|
||||
RobotConfig& config
|
||||
);
|
||||
|
||||
// Servo manager class for cleaner access
|
||||
class ServoManager {
|
||||
public:
|
||||
void init();
|
||||
|
||||
Feetech* channel(uint8_t ch) { return servos[ch]; }
|
||||
Feetech* operator[](uint8_t ch) { return servos[ch]; }
|
||||
|
||||
// Convenience methods
|
||||
void syncWritePositions(
|
||||
const uint8_t* ids,
|
||||
uint16_t* positions,
|
||||
uint16_t* speeds,
|
||||
uint8_t count,
|
||||
RobotConfig& config,
|
||||
uint8_t channel = 0
|
||||
);
|
||||
|
||||
private:
|
||||
Feetech* servos[2] = {nullptr, nullptr};
|
||||
};
|
||||
|
||||
// Global servo manager
|
||||
extern ServoManager servoManager;
|
||||
|
||||
|
|
@ -1,13 +1,10 @@
|
|||
#include "nodegraph.h"
|
||||
#include "animation.h"
|
||||
#include "noise.h"
|
||||
#include "robotconfig.h"
|
||||
#include "sensors.h"
|
||||
#include "RobotConfig.h"
|
||||
#include <cstring>
|
||||
|
||||
extern RobotConfig config;
|
||||
extern ADXL345 adxl;
|
||||
extern Radar radar;
|
||||
|
||||
|
||||
// CurveNode evaluation
|
||||
|
|
@ -64,79 +61,6 @@ void VariableNode::evaluate(uint32_t currentTick) {
|
|||
case VAR_SERVO_FEEDBACK:
|
||||
outputValue = config.getMotorPosition(arg0);
|
||||
break;
|
||||
|
||||
// ADXL345 accelerometer sources - scale to 0-4095 range
|
||||
case VAR_ACCEL_X:
|
||||
// X acceleration: -2g to +2g → 0-4095 (2048 = 0g)
|
||||
if (adxl.isReady()) {
|
||||
float accel = constrain(adxl.getAccelX(), -2.0f, 2.0f);
|
||||
outputValue = (uint16_t)(((accel + 2.0f) / 4.0f) * 4095.0f);
|
||||
} else {
|
||||
outputValue = 2048;
|
||||
}
|
||||
break;
|
||||
case VAR_ACCEL_Y:
|
||||
// Y acceleration: -2g to +2g → 0-4095 (2048 = 0g)
|
||||
if (adxl.isReady()) {
|
||||
float accel = constrain(adxl.getAccelY(), -2.0f, 2.0f);
|
||||
outputValue = (uint16_t)(((accel + 2.0f) / 4.0f) * 4095.0f);
|
||||
} else {
|
||||
outputValue = 2048;
|
||||
}
|
||||
break;
|
||||
case VAR_ACCEL_Z:
|
||||
// Z acceleration: -2g to +2g → 0-4095 (2048 = 0g)
|
||||
if (adxl.isReady()) {
|
||||
float accel = constrain(adxl.getAccelZ(), -2.0f, 2.0f);
|
||||
outputValue = (uint16_t)(((accel + 2.0f) / 4.0f) * 4095.0f);
|
||||
} else {
|
||||
outputValue = 2048;
|
||||
}
|
||||
break;
|
||||
case VAR_TILT_PITCH:
|
||||
// Pitch angle: -90° to +90° → 0-4095 (2048 = level)
|
||||
if (adxl.isReady()) {
|
||||
float pitch = constrain(adxl.getPitch(), -90.0f, 90.0f);
|
||||
outputValue = (uint16_t)(((pitch + 90.0f) / 180.0f) * 4095.0f);
|
||||
} else {
|
||||
outputValue = 2048;
|
||||
}
|
||||
break;
|
||||
case VAR_TILT_ROLL:
|
||||
// Roll angle: -90° to +90° → 0-4095 (2048 = level)
|
||||
if (adxl.isReady()) {
|
||||
float roll = constrain(adxl.getRoll(), -90.0f, 90.0f);
|
||||
outputValue = (uint16_t)(((roll + 90.0f) / 180.0f) * 4095.0f);
|
||||
} else {
|
||||
outputValue = 2048;
|
||||
}
|
||||
break;
|
||||
|
||||
// Radar sources - primary target (index 0)
|
||||
case VAR_RADAR_X:
|
||||
// X position: -200cm to +200cm → 0-4095 (2048 = center)
|
||||
{
|
||||
const RadarTarget& target = radar.getTarget(0);
|
||||
if (target.valid) {
|
||||
float x = constrain(target.x, -200.0f, 200.0f);
|
||||
outputValue = (uint16_t)(((x + 200.0f) / 400.0f) * 4095.0f);
|
||||
} else {
|
||||
outputValue = 2048; // Center if no target
|
||||
}
|
||||
}
|
||||
break;
|
||||
case VAR_RADAR_Y:
|
||||
// Y distance: 0-500cm → 0-4095
|
||||
{
|
||||
const RadarTarget& target = radar.getTarget(0);
|
||||
if (target.valid) {
|
||||
float y = constrain(target.y, 0.0f, 500.0f);
|
||||
outputValue = (uint16_t)((y / 500.0f) * 4095.0f);
|
||||
} else {
|
||||
outputValue = 0; // No target = 0 distance
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
11
nodegraph.h
11
nodegraph.h
|
|
@ -19,16 +19,7 @@ enum VariableSource {
|
|||
VAR_FACE_Y,
|
||||
VAR_SINE,
|
||||
VAR_ANALOG,
|
||||
VAR_SERVO_FEEDBACK,
|
||||
// ADXL345 accelerometer sources
|
||||
VAR_ACCEL_X, // X acceleration → 0-4095 (-2g to +2g, 2048 = 0g)
|
||||
VAR_ACCEL_Y, // Y acceleration → 0-4095 (-2g to +2g, 2048 = 0g)
|
||||
VAR_ACCEL_Z, // Z acceleration → 0-4095 (-2g to +2g, 2048 = 0g)
|
||||
VAR_TILT_PITCH, // Pitch angle from accel → 0-4095 (-90° to +90°, 2048 = level)
|
||||
VAR_TILT_ROLL, // Roll angle from accel → 0-4095 (-90° to +90°, 2048 = level)
|
||||
// Radar sources (RD-03D) - primary target
|
||||
VAR_RADAR_X, // X position (angle) → 0-4095 (2048 = center)
|
||||
VAR_RADAR_Y // Y distance → 0-4095 (scaled 0-500cm)
|
||||
VAR_SERVO_FEEDBACK
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
209
protocol.cpp
209
protocol.cpp
|
|
@ -1,209 +0,0 @@
|
|||
#include "protocol.h"
|
||||
|
||||
// ============================================================================
|
||||
// Global Buffers
|
||||
// ============================================================================
|
||||
|
||||
uint8_t g_rxBuffer[MAX_PAYLOAD_SIZE + PACKET_HEADER_SIZE + PACKET_CRC_SIZE];
|
||||
uint16_t g_rxBufferLen = 0;
|
||||
|
||||
// Parsed packet info
|
||||
static char s_rxTag[4];
|
||||
static uint16_t s_rxPayloadLen = 0;
|
||||
static uint16_t s_rxSeq = 0;
|
||||
static uint16_t s_rxPayloadOffset = 0;
|
||||
|
||||
// Sequence counters (per-tag would be ideal, but global is simpler)
|
||||
static uint16_t s_txSeq = 0;
|
||||
|
||||
// Receive state machine
|
||||
enum RxState { RX_SYNC0, RX_SYNC1, RX_HEADER, RX_PAYLOAD, RX_CRC };
|
||||
static RxState s_rxState = RX_SYNC0;
|
||||
static uint16_t s_rxIdx = 0;
|
||||
static uint16_t s_rxExpectedLen = 0;
|
||||
|
||||
// ============================================================================
|
||||
// CRC16-CCITT (polynomial 0x1021, init 0xFFFF)
|
||||
// ============================================================================
|
||||
|
||||
uint16_t crc16Update(uint16_t crc, const uint8_t* data, uint16_t len) {
|
||||
for (uint16_t i = 0; i < len; i++) {
|
||||
crc ^= (uint16_t)data[i] << 8;
|
||||
for (uint8_t j = 0; j < 8; j++) {
|
||||
if (crc & 0x8000) {
|
||||
crc = (crc << 1) ^ 0x1021;
|
||||
} else {
|
||||
crc <<= 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
uint16_t crc16Compute(const uint8_t* data, uint16_t len) {
|
||||
return crc16Update(0xFFFF, data, len);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Packet Sending
|
||||
// ============================================================================
|
||||
|
||||
void sendPacket(const char tag[4], const uint8_t* payload, uint16_t len) {
|
||||
// Build header
|
||||
uint8_t header[PACKET_HEADER_SIZE];
|
||||
header[0] = SYNC0;
|
||||
header[1] = SYNC1;
|
||||
header[2] = tag[0];
|
||||
header[3] = tag[1];
|
||||
header[4] = tag[2];
|
||||
header[5] = tag[3];
|
||||
header[6] = len & 0xFF;
|
||||
header[7] = (len >> 8) & 0xFF;
|
||||
header[8] = s_txSeq & 0xFF;
|
||||
header[9] = (s_txSeq >> 8) & 0xFF;
|
||||
|
||||
// Compute CRC over tag + len + seq + payload
|
||||
uint16_t crc = 0xFFFF;
|
||||
crc = crc16Update(crc, header + 2, 8); // tag(4) + len(2) + seq(2)
|
||||
if (len > 0 && payload != nullptr) {
|
||||
crc = crc16Update(crc, payload, len);
|
||||
}
|
||||
|
||||
// Send
|
||||
Serial.write(header, PACKET_HEADER_SIZE);
|
||||
if (len > 0 && payload != nullptr) {
|
||||
Serial.write(payload, len);
|
||||
}
|
||||
Serial.write(crc & 0xFF);
|
||||
Serial.write((crc >> 8) & 0xFF);
|
||||
|
||||
s_txSeq++;
|
||||
}
|
||||
|
||||
void sendMessage(const String& msg) {
|
||||
sendPacket(Tag::MSGE, (const uint8_t*)msg.c_str(), msg.length());
|
||||
}
|
||||
|
||||
void sendAck(const char originalTag[4]) {
|
||||
uint8_t payload[4];
|
||||
memcpy(payload, originalTag, 4);
|
||||
sendPacket(Tag::ACK, payload, 4);
|
||||
}
|
||||
|
||||
void sendNack(const char originalTag[4], const String& reason) {
|
||||
uint8_t payload[4 + 64];
|
||||
memcpy(payload, originalTag, 4);
|
||||
uint16_t len = 4;
|
||||
|
||||
if (reason.length() > 0) {
|
||||
uint16_t reasonLen = min((uint16_t)reason.length(), (uint16_t)60);
|
||||
memcpy(payload + 4, reason.c_str(), reasonLen);
|
||||
len += reasonLen;
|
||||
}
|
||||
|
||||
sendPacket(Tag::NACK, payload, len);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Packet Receiving
|
||||
// ============================================================================
|
||||
|
||||
bool receivePacket() {
|
||||
while (Serial.available()) {
|
||||
uint8_t b = Serial.read();
|
||||
|
||||
switch (s_rxState) {
|
||||
case RX_SYNC0:
|
||||
if (b == SYNC0) {
|
||||
g_rxBuffer[0] = b;
|
||||
s_rxState = RX_SYNC1;
|
||||
}
|
||||
break;
|
||||
|
||||
case RX_SYNC1:
|
||||
if (b == SYNC1) {
|
||||
g_rxBuffer[1] = b;
|
||||
s_rxIdx = 2;
|
||||
s_rxState = RX_HEADER;
|
||||
} else if (b == SYNC0) {
|
||||
// Stay in SYNC1 state, might be repeated sync
|
||||
} else {
|
||||
s_rxState = RX_SYNC0;
|
||||
}
|
||||
break;
|
||||
|
||||
case RX_HEADER:
|
||||
g_rxBuffer[s_rxIdx++] = b;
|
||||
if (s_rxIdx >= PACKET_HEADER_SIZE) {
|
||||
// Parse header
|
||||
memcpy(s_rxTag, g_rxBuffer + 2, 4);
|
||||
s_rxPayloadLen = g_rxBuffer[6] | (g_rxBuffer[7] << 8);
|
||||
s_rxSeq = g_rxBuffer[8] | (g_rxBuffer[9] << 8);
|
||||
s_rxPayloadOffset = PACKET_HEADER_SIZE;
|
||||
|
||||
// Validate length
|
||||
if (s_rxPayloadLen > MAX_PAYLOAD_SIZE) {
|
||||
Serial.println("Packet too large");
|
||||
s_rxState = RX_SYNC0;
|
||||
break;
|
||||
}
|
||||
|
||||
s_rxExpectedLen = PACKET_HEADER_SIZE + s_rxPayloadLen + PACKET_CRC_SIZE;
|
||||
|
||||
if (s_rxPayloadLen == 0) {
|
||||
s_rxState = RX_CRC;
|
||||
} else {
|
||||
s_rxState = RX_PAYLOAD;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case RX_PAYLOAD:
|
||||
g_rxBuffer[s_rxIdx++] = b;
|
||||
if (s_rxIdx >= PACKET_HEADER_SIZE + s_rxPayloadLen) {
|
||||
s_rxState = RX_CRC;
|
||||
}
|
||||
break;
|
||||
|
||||
case RX_CRC:
|
||||
g_rxBuffer[s_rxIdx++] = b;
|
||||
if (s_rxIdx >= s_rxExpectedLen) {
|
||||
// Verify CRC
|
||||
uint16_t receivedCrc = g_rxBuffer[s_rxIdx - 2] | (g_rxBuffer[s_rxIdx - 1] << 8);
|
||||
uint16_t computedCrc = crc16Compute(g_rxBuffer + 2, s_rxPayloadLen + 8);
|
||||
|
||||
s_rxState = RX_SYNC0;
|
||||
g_rxBufferLen = s_rxIdx;
|
||||
|
||||
if (receivedCrc == computedCrc) {
|
||||
return true; // Valid packet ready
|
||||
} else {
|
||||
Serial.println("CRC mismatch");
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
const char* getReceivedTag() {
|
||||
return s_rxTag;
|
||||
}
|
||||
|
||||
const uint8_t* getReceivedPayload() {
|
||||
return g_rxBuffer + s_rxPayloadOffset;
|
||||
}
|
||||
|
||||
uint16_t getReceivedPayloadLen() {
|
||||
return s_rxPayloadLen;
|
||||
}
|
||||
|
||||
uint16_t getReceivedSeq() {
|
||||
return s_rxSeq;
|
||||
}
|
||||
|
||||
bool tagMatches(const char* received, const char expected[4]) {
|
||||
return memcmp(received, expected, 4) == 0;
|
||||
}
|
||||
131
protocol.h
131
protocol.h
|
|
@ -1,131 +0,0 @@
|
|||
#pragma once
|
||||
#include <Arduino.h>
|
||||
|
||||
// ============================================================================
|
||||
// Protocol Constants
|
||||
// ============================================================================
|
||||
|
||||
// Sync bytes (distinguishable from Feetech 0xFF 0xFF)
|
||||
constexpr uint8_t SYNC0 = 0xA5;
|
||||
constexpr uint8_t SYNC1 = 0x5A;
|
||||
|
||||
// Packet structure:
|
||||
// [SYNC0][SYNC1][TAG 4 bytes][LENGTH 2 bytes][SEQ 2 bytes][PAYLOAD...][CRC16 2 bytes]
|
||||
// Total overhead: 12 bytes
|
||||
|
||||
constexpr uint16_t MAX_PAYLOAD_SIZE = 6000;
|
||||
constexpr uint16_t PACKET_HEADER_SIZE = 10; // sync(2) + tag(4) + len(2) + seq(2)
|
||||
constexpr uint16_t PACKET_CRC_SIZE = 2;
|
||||
|
||||
// ============================================================================
|
||||
// Packet Tags (4 bytes each, human-readable)
|
||||
// ============================================================================
|
||||
|
||||
namespace Tag {
|
||||
// Identity & Config
|
||||
constexpr char IDENT[4] = {'I','D','N','T'}; // Identity request/response
|
||||
constexpr char CONFG[4] = {'C','O','N','F'}; // Config update
|
||||
|
||||
// File Operations
|
||||
constexpr char FLIST[4] = {'F','L','S','T'}; // File list
|
||||
constexpr char FLOAD[4] = {'F','L','O','D'}; // File load
|
||||
constexpr char FSAVE[4] = {'F','S','A','V'}; // File save
|
||||
constexpr char FDELE[4] = {'F','D','E','L'}; // File delete
|
||||
constexpr char FPLAY[4] = {'F','P','L','Y'}; // Play animation file
|
||||
constexpr char FSTP[4] = {'F','S','T','P'}; // Stop animation
|
||||
|
||||
// Motor Control
|
||||
constexpr char MSET[4] = {'M','S','E','T'}; // Set motor positions
|
||||
constexpr char MPOS[4] = {'M','P','O','S'}; // Motor position stream
|
||||
constexpr char MSCAN[4] = {'M','S','C','N'}; // Scan for motors
|
||||
constexpr char MWRIT[4] = {'M','W','R','T'}; // Write motor register
|
||||
constexpr char MSTRM[4] = {'M','S','T','M'}; // Motor stream control
|
||||
|
||||
// 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 FRAME[4] = {'F','R','M','E'}; // Animation frame events (frame, mode, status)
|
||||
|
||||
// Behaviors
|
||||
constexpr char BHVR[4] = {'B','H','V','R'}; // Behavior control (activate/deactivate)
|
||||
constexpr char BLST[4] = {'B','L','S','T'}; // Behavior list (list all behaviors and their states)
|
||||
|
||||
// Visemes
|
||||
constexpr char VLST[4] = {'V','L','S','T'}; // List all visemes
|
||||
constexpr char VADD[4] = {'V','A','D','D'}; // Add a new viseme
|
||||
constexpr char VDEL[4] = {'V','D','E','L'}; // Delete a viseme
|
||||
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
|
||||
|
||||
// System
|
||||
constexpr char STATE[4] = {'S','T','A','T'}; // System state/heartbeat
|
||||
constexpr char MSGE[4] = {'M','S','G','E'}; // Log/debug message
|
||||
constexpr char BOOT[4] = {'B','O','O','T'}; // Enter bootloader
|
||||
constexpr char ACK[4] = {'A','C','K','!'}; // Acknowledge
|
||||
constexpr char NACK[4] = {'N','A','C','K'}; // Negative acknowledge
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Play Modes (for FPLAY command)
|
||||
// ============================================================================
|
||||
|
||||
enum PlayMode : uint8_t {
|
||||
PLAY_IDLE = 0x00,
|
||||
PLAY_ONCE = 0x01,
|
||||
PLAY_LOOP = 0x02,
|
||||
PLAY_REPEAT = 0x03
|
||||
};
|
||||
|
||||
// ============================================================================
|
||||
// Packet Buffer
|
||||
// ============================================================================
|
||||
|
||||
extern uint8_t g_rxBuffer[MAX_PAYLOAD_SIZE + PACKET_HEADER_SIZE + PACKET_CRC_SIZE];
|
||||
extern uint16_t g_rxBufferLen;
|
||||
|
||||
// ============================================================================
|
||||
// CRC16-CCITT
|
||||
// ============================================================================
|
||||
|
||||
uint16_t crc16Update(uint16_t crc, const uint8_t* data, uint16_t len);
|
||||
uint16_t crc16Compute(const uint8_t* data, uint16_t len);
|
||||
|
||||
// ============================================================================
|
||||
// Packet Sending
|
||||
// ============================================================================
|
||||
|
||||
// Send a tagged packet with auto-incrementing sequence number
|
||||
void sendPacket(const char tag[4], const uint8_t* payload, uint16_t len);
|
||||
|
||||
// Convenience: send string as MSGE packet
|
||||
void sendMessage(const String& msg);
|
||||
|
||||
// Convenience: send ACK/NACK
|
||||
void sendAck(const char originalTag[4]);
|
||||
void sendNack(const char originalTag[4], const String& reason = "");
|
||||
|
||||
// ============================================================================
|
||||
// Packet Receiving
|
||||
// ============================================================================
|
||||
|
||||
// Packet receive state machine - call from loop()
|
||||
// Returns true when a complete valid packet is ready
|
||||
bool receivePacket();
|
||||
|
||||
// Get received packet info (valid after receivePacket returns true)
|
||||
const char* getReceivedTag();
|
||||
const uint8_t* getReceivedPayload();
|
||||
uint16_t getReceivedPayloadLen();
|
||||
uint16_t getReceivedSeq();
|
||||
|
||||
// Check if received tag matches
|
||||
bool tagMatches(const char* received, const char expected[4]);
|
||||
|
||||
// ============================================================================
|
||||
// Utility
|
||||
// ============================================================================
|
||||
|
||||
// Compare tags
|
||||
inline bool tagsEqual(const char a[4], const char b[4]) {
|
||||
return memcmp(a, b, 4) == 0;
|
||||
}
|
||||
327
robotconfig.cpp
327
robotconfig.cpp
|
|
@ -1,5 +1,4 @@
|
|||
#include "robotconfig.h"
|
||||
#include "behaviors.h"
|
||||
#include "RobotConfig.h"
|
||||
#include <FFat.h>
|
||||
|
||||
uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
|
||||
|
|
@ -12,15 +11,6 @@ uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
|
|||
return 2047;
|
||||
}
|
||||
|
||||
uint16_t RobotConfig::getMotorCurrent(uint8_t motorID) const {
|
||||
for (const Motor& motor : motors) {
|
||||
if (motor.motorID == motorID) {
|
||||
return motor.current;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint8_t RobotConfig::getMotorModel(uint8_t motorID) {
|
||||
for (const Motor& motor : motors) {
|
||||
if (motor.motorID == motorID) {
|
||||
|
|
@ -42,16 +32,6 @@ bool RobotConfig::setMotorPosition(uint8_t motorID, uint16_t newPosition) {
|
|||
return false;
|
||||
}
|
||||
|
||||
bool RobotConfig::setMotorCurrent(uint8_t motorID, uint16_t newCurrent) {
|
||||
for (Motor& motor : motors) {
|
||||
if (motor.motorID == motorID) {
|
||||
motor.current = newCurrent;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool RobotConfig::setMotorEnabled(uint8_t motorID, bool enable) {
|
||||
for (Motor& motor : motors) {
|
||||
if (motor.motorID == motorID) {
|
||||
|
|
@ -218,310 +198,10 @@ bool RobotConfig::loadFromFFat(const char* path) {
|
|||
return true;
|
||||
}
|
||||
|
||||
// Legacy method - calls new version with null pointers
|
||||
bool RobotConfig::loadOrCreateDefault(const char* path) {
|
||||
return loadOrCreateDefault(path, nullptr, nullptr);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Key-Value Format V2 (Compact, Extensible)
|
||||
// ============================================================================
|
||||
|
||||
bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) const {
|
||||
File file = FFat.open(path, FILE_WRITE);
|
||||
if (!file) return false;
|
||||
|
||||
// Write version header
|
||||
file.write((uint8_t)0x02); // Version 2
|
||||
|
||||
// Count settings (we'll write this after we know the count)
|
||||
uint16_t settingCount = 0;
|
||||
size_t countPos = file.position();
|
||||
file.write((uint8_t)0); // Placeholder for count low byte
|
||||
file.write((uint8_t)0); // Placeholder for count high byte
|
||||
|
||||
// Setting 1: Device Name
|
||||
if (deviceName.length() > 0) {
|
||||
file.write((uint8_t)(KEY_DEVICE_NAME & 0xFF));
|
||||
file.write((uint8_t)((KEY_DEVICE_NAME >> 8) & 0xFF));
|
||||
file.write(TYPE_STRING);
|
||||
uint8_t nameLen = deviceName.length();
|
||||
file.write(nameLen);
|
||||
file.write((const uint8_t*)deviceName.c_str(), nameLen);
|
||||
settingCount++;
|
||||
}
|
||||
|
||||
// Setting 2: Firmware Major
|
||||
file.write((uint8_t)(KEY_FIRMWARE_MAJOR & 0xFF));
|
||||
file.write((uint8_t)((KEY_FIRMWARE_MAJOR >> 8) & 0xFF));
|
||||
file.write(TYPE_UINT8);
|
||||
file.write(firmwareVersion.major);
|
||||
settingCount++;
|
||||
|
||||
// Setting 3: Firmware Minor
|
||||
file.write((uint8_t)(KEY_FIRMWARE_MINOR & 0xFF));
|
||||
file.write((uint8_t)((KEY_FIRMWARE_MINOR >> 8) & 0xFF));
|
||||
file.write(TYPE_UINT8);
|
||||
file.write(firmwareVersion.minor);
|
||||
settingCount++;
|
||||
|
||||
// Setting 4: Motor Array
|
||||
if (!motors.empty()) {
|
||||
file.write((uint8_t)(KEY_MOTOR_ARRAY & 0xFF));
|
||||
file.write((uint8_t)((KEY_MOTOR_ARRAY >> 8) & 0xFF));
|
||||
file.write(TYPE_MOTOR_ARRAY);
|
||||
|
||||
uint8_t motorCount = motors.size();
|
||||
file.write(motorCount);
|
||||
|
||||
for (const Motor& m : motors) {
|
||||
file.write(m.motorID);
|
||||
file.write(m.servoModel.major);
|
||||
file.write(m.servoModel.minor);
|
||||
|
||||
uint8_t nameLen = m.name.length();
|
||||
file.write(nameLen);
|
||||
if (nameLen > 0) {
|
||||
file.write((const uint8_t*)m.name.c_str(), nameLen);
|
||||
}
|
||||
}
|
||||
settingCount++;
|
||||
}
|
||||
|
||||
// Setting 5: Behavior States
|
||||
if (behaviorManager) {
|
||||
file.write((uint8_t)(KEY_BEHAVIOR_STATES & 0xFF));
|
||||
file.write((uint8_t)((KEY_BEHAVIOR_STATES >> 8) & 0xFF));
|
||||
file.write(TYPE_BEHAVIOR_STATES);
|
||||
|
||||
// Count enabled behaviors
|
||||
uint8_t behaviorCount = behaviorManager->getBehaviorCount();
|
||||
file.write(behaviorCount);
|
||||
|
||||
// Write behaviorID + enabled state pairs
|
||||
for (uint8_t i = 0; i < behaviorCount; i++) {
|
||||
BehaviorID id;
|
||||
bool enabled;
|
||||
if (behaviorManager->getBehaviorInfo(i, id, enabled)) {
|
||||
file.write((uint8_t)id);
|
||||
file.write(enabled ? 1 : 0);
|
||||
}
|
||||
}
|
||||
settingCount++;
|
||||
}
|
||||
|
||||
// Setting 6: Viseme Array
|
||||
if (visemeBehavior) {
|
||||
const std::vector<Viseme>& visemes = visemeBehavior->getVisemes();
|
||||
if (!visemes.empty()) {
|
||||
file.write((uint8_t)(KEY_VISEME_ARRAY & 0xFF));
|
||||
file.write((uint8_t)((KEY_VISEME_ARRAY >> 8) & 0xFF));
|
||||
file.write(TYPE_VISEME_ARRAY);
|
||||
|
||||
uint8_t visemeCount = visemes.size();
|
||||
file.write(visemeCount);
|
||||
|
||||
for (const Viseme& v : visemes) {
|
||||
file.write(v.id);
|
||||
|
||||
// Label (3 bytes)
|
||||
file.write((uint8_t)v.label[0]);
|
||||
file.write((uint8_t)v.label[1]);
|
||||
file.write((uint8_t)v.label[2]);
|
||||
|
||||
// Motor positions
|
||||
uint8_t motorCount = v.motorPositions.size();
|
||||
file.write(motorCount);
|
||||
|
||||
for (const VisemeMotorPosition& mp : v.motorPositions) {
|
||||
file.write(mp.motorID);
|
||||
file.write((uint8_t)(mp.position & 0xFF)); // position low
|
||||
file.write((uint8_t)((mp.position >> 8) & 0xFF)); // position high
|
||||
}
|
||||
}
|
||||
settingCount++;
|
||||
}
|
||||
}
|
||||
|
||||
// Write setting count at the beginning
|
||||
size_t endPos = file.position();
|
||||
file.seek(countPos);
|
||||
file.write((uint8_t)(settingCount & 0xFF));
|
||||
file.write((uint8_t)((settingCount >> 8) & 0xFF));
|
||||
file.seek(endPos);
|
||||
|
||||
file.close();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) {
|
||||
File file = FFat.open(path, FILE_READ);
|
||||
if (!file) return false;
|
||||
|
||||
// Read version
|
||||
uint8_t version = file.read();
|
||||
if (version != 0x02) {
|
||||
file.close();
|
||||
return false; // Wrong version
|
||||
}
|
||||
|
||||
// Read setting count
|
||||
uint16_t settingCount = file.read();
|
||||
settingCount |= (file.read() << 8);
|
||||
|
||||
motors.clear();
|
||||
|
||||
// Parse each setting
|
||||
for (uint16_t i = 0; i < settingCount; i++) {
|
||||
// Read key
|
||||
uint16_t key = file.read();
|
||||
key |= (file.read() << 8);
|
||||
|
||||
// Read type
|
||||
uint8_t type = file.read();
|
||||
|
||||
switch (key) {
|
||||
case KEY_DEVICE_NAME: {
|
||||
if (type == TYPE_STRING) {
|
||||
uint8_t nameLen = file.read();
|
||||
char nameBuf[64] = {0};
|
||||
if (nameLen > 0 && nameLen < sizeof(nameBuf)) {
|
||||
file.readBytes(nameBuf, nameLen);
|
||||
deviceName = String(nameBuf);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case KEY_FIRMWARE_MAJOR: {
|
||||
if (type == TYPE_UINT8) {
|
||||
firmwareVersion.major = file.read();
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case KEY_FIRMWARE_MINOR: {
|
||||
if (type == TYPE_UINT8) {
|
||||
firmwareVersion.minor = file.read();
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case KEY_MOTOR_ARRAY: {
|
||||
if (type == TYPE_MOTOR_ARRAY) {
|
||||
uint8_t motorCount = file.read();
|
||||
|
||||
for (uint8_t j = 0; j < motorCount; j++) {
|
||||
Motor m;
|
||||
m.motorID = file.read();
|
||||
m.servoModel.major = file.read();
|
||||
m.servoModel.minor = file.read();
|
||||
|
||||
uint8_t nameLen = file.read();
|
||||
char nameBuf[64] = {0};
|
||||
if (nameLen > 0 && nameLen < sizeof(nameBuf)) {
|
||||
file.readBytes(nameBuf, nameLen);
|
||||
m.name = String(nameBuf);
|
||||
}
|
||||
|
||||
m.position = 0;
|
||||
m.isEnabled = true;
|
||||
motors.push_back(m);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case KEY_BEHAVIOR_STATES: {
|
||||
if (type == TYPE_BEHAVIOR_STATES && behaviorManager) {
|
||||
uint8_t behaviorCount = file.read();
|
||||
|
||||
for (uint8_t j = 0; j < behaviorCount; j++) {
|
||||
BehaviorID id = (BehaviorID)file.read();
|
||||
bool enabled = file.read() != 0;
|
||||
behaviorManager->setBehaviorEnabled(id, enabled);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case KEY_VISEME_ARRAY: {
|
||||
if (type == TYPE_VISEME_ARRAY && visemeBehavior) {
|
||||
uint8_t visemeCount = file.read();
|
||||
|
||||
for (uint8_t j = 0; j < visemeCount; j++) {
|
||||
uint8_t visemeID = file.read();
|
||||
|
||||
// Read label (3 bytes)
|
||||
char label[4];
|
||||
label[0] = file.read();
|
||||
label[1] = file.read();
|
||||
label[2] = file.read();
|
||||
label[3] = '\0';
|
||||
|
||||
// Read motor positions
|
||||
uint8_t motorCount = file.read();
|
||||
std::vector<VisemeMotorPosition> positions;
|
||||
|
||||
for (uint8_t k = 0; k < motorCount; k++) {
|
||||
VisemeMotorPosition mp;
|
||||
mp.motorID = file.read();
|
||||
uint16_t posLow = file.read();
|
||||
uint16_t posHigh = file.read();
|
||||
mp.position = posLow | (posHigh << 8);
|
||||
positions.push_back(mp);
|
||||
}
|
||||
|
||||
// Create or update viseme
|
||||
visemeBehavior->createOrUpdateViseme(visemeID, label, positions);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
// Unknown key - skip based on type
|
||||
switch (type) {
|
||||
case TYPE_UINT8: file.read(); break;
|
||||
case TYPE_UINT16: file.read(); file.read(); break;
|
||||
case TYPE_UINT32: file.read(); file.read(); file.read(); file.read(); break;
|
||||
case TYPE_BOOL: file.read(); break;
|
||||
case TYPE_INT8: file.read(); break;
|
||||
case TYPE_INT16: file.read(); file.read(); break;
|
||||
case TYPE_INT32: file.read(); file.read(); file.read(); file.read(); break;
|
||||
case TYPE_FLOAT: file.read(); file.read(); file.read(); file.read(); break;
|
||||
case TYPE_STRING: {
|
||||
uint8_t len = file.read();
|
||||
for (uint8_t k = 0; k < len; k++) file.read();
|
||||
break;
|
||||
}
|
||||
default:
|
||||
file.close();
|
||||
return false; // Unknown type
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
file.close();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool RobotConfig::loadOrCreateDefault(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) {
|
||||
if (FFat.exists(path)) {
|
||||
Serial.println("Loading robot config from FFat...");
|
||||
// Try V2 format first
|
||||
if (loadFromFFatV2(path, behaviorManager, visemeBehavior)) {
|
||||
Serial.println("Loaded V2 format");
|
||||
return true;
|
||||
}
|
||||
// Fall back to V1 format
|
||||
if (loadFromFFat(path)) {
|
||||
Serial.println("Loaded V1 format (legacy)");
|
||||
// Upgrade to V2 format
|
||||
saveToFFatV2(path, behaviorManager, visemeBehavior);
|
||||
return true;
|
||||
}
|
||||
return loadFromFFat(path);
|
||||
}
|
||||
|
||||
Serial.println("No config found. Creating default config...");
|
||||
|
|
@ -529,7 +209,8 @@ bool RobotConfig::loadOrCreateDefault(const char* path, BehaviorManager* behavio
|
|||
// 🔧 Define your default config here
|
||||
deviceName = "DefaultBot";
|
||||
firmwareVersion = { 1, 0 };
|
||||
|
||||
motors.clear();
|
||||
|
||||
return saveToFFatV2(path, behaviorManager, visemeBehavior);
|
||||
return saveToFFat(path);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2,50 +2,6 @@
|
|||
#include <Arduino.h>
|
||||
#include <vector>
|
||||
|
||||
// Forward declarations
|
||||
class BehaviorManager;
|
||||
class VisemeBehavior;
|
||||
|
||||
// ============================================================================
|
||||
// Config Key-Value System
|
||||
// ============================================================================
|
||||
|
||||
enum ConfigKey : uint16_t {
|
||||
// Basic settings
|
||||
KEY_DEVICE_NAME = 0x0001,
|
||||
KEY_FIRMWARE_MAJOR = 0x0002,
|
||||
KEY_FIRMWARE_MINOR = 0x0003,
|
||||
|
||||
// Motor array (single entry containing all motors)
|
||||
KEY_MOTOR_ARRAY = 0x0100,
|
||||
|
||||
// Behavior states (array of behaviorID + enabled pairs)
|
||||
KEY_BEHAVIOR_STATES = 0x0200,
|
||||
|
||||
// Viseme array (single entry containing all visemes)
|
||||
KEY_VISEME_ARRAY = 0x0300,
|
||||
|
||||
// Future extensible settings
|
||||
KEY_SERIAL_BAUD = 0x0400,
|
||||
KEY_MOTOR_UPDATE_INTERVAL = 0x0401,
|
||||
// ... add more as needed
|
||||
};
|
||||
|
||||
enum ConfigType : uint8_t {
|
||||
TYPE_UINT8 = 0x01,
|
||||
TYPE_UINT16 = 0x02,
|
||||
TYPE_UINT32 = 0x03,
|
||||
TYPE_BOOL = 0x04,
|
||||
TYPE_INT8 = 0x05,
|
||||
TYPE_INT16 = 0x06,
|
||||
TYPE_INT32 = 0x07,
|
||||
TYPE_FLOAT = 0x08,
|
||||
TYPE_STRING = 0x09,
|
||||
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
|
||||
};
|
||||
|
||||
struct FirmwareVersion {
|
||||
uint8_t major;
|
||||
uint8_t minor;
|
||||
|
|
@ -61,7 +17,6 @@ struct Motor {
|
|||
uint8_t motorID;
|
||||
ServoModel servoModel;
|
||||
uint16_t position;
|
||||
uint16_t current = 0;
|
||||
bool isEnabled = true;
|
||||
};
|
||||
|
||||
|
|
@ -71,34 +26,16 @@ struct RobotConfig {
|
|||
std::vector<Motor> motors;
|
||||
|
||||
uint16_t getMotorPosition(uint8_t motorID) const;
|
||||
uint16_t getMotorCurrent(uint8_t motorID) const;
|
||||
uint8_t getMotorModel(uint8_t motorID);
|
||||
bool setMotorPosition(uint8_t motorID, uint16_t newPosition);
|
||||
bool setMotorCurrent(uint8_t motorID, uint16_t newCurrent);
|
||||
bool setMotorEnabled(uint8_t motorID, bool enable);
|
||||
bool isMotorEnabled(uint8_t motorID);
|
||||
void enableAllMotors();
|
||||
String serializeJSON() const;
|
||||
std::vector<uint8_t> serializeToBytes() const;
|
||||
|
||||
// Legacy format (v1)
|
||||
bool saveToFFat(const char* path = "/robot_config.bin") const;
|
||||
bool loadFromFFat(const char* path = "/robot_config.bin");
|
||||
|
||||
// New key-value format (v2)
|
||||
bool saveToFFatV2(const char* path = "/robot_config.bin",
|
||||
BehaviorManager* behaviorManager = nullptr,
|
||||
VisemeBehavior* visemeBehavior = nullptr) const;
|
||||
bool loadFromFFatV2(const char* path = "/robot_config.bin",
|
||||
BehaviorManager* behaviorManager = nullptr,
|
||||
VisemeBehavior* visemeBehavior = nullptr);
|
||||
|
||||
// New version with behavior/viseme support
|
||||
bool loadOrCreateDefault(const char* path = "/robot_config.bin",
|
||||
BehaviorManager* behaviorManager = nullptr,
|
||||
VisemeBehavior* visemeBehavior = nullptr);
|
||||
|
||||
// Legacy version (for backward compatibility)
|
||||
bool loadOrCreateDefault(const char* path);
|
||||
bool loadOrCreateDefault(const char* path = "/robot_config.bin");
|
||||
|
||||
};
|
||||
|
|
|
|||
340
sensors.cpp
340
sensors.cpp
|
|
@ -1,340 +0,0 @@
|
|||
#include "sensors.h"
|
||||
#include "protocol.h"
|
||||
|
||||
// ============================================================================
|
||||
// Global Instances
|
||||
// ============================================================================
|
||||
|
||||
Radar radar;
|
||||
ADXL345 adxl;
|
||||
SensorManager sensors;
|
||||
|
||||
// ============================================================================
|
||||
// Radar Implementation
|
||||
// ============================================================================
|
||||
|
||||
static const uint8_t RADAR_HEADER[] = {0xAA, 0xFF, 0x03, 0x00};
|
||||
static const uint8_t RADAR_FOOTER[] = {0x55, 0xCC};
|
||||
constexpr float RADAR_DISTANCE_SCALE = 0.1f; // Raw mm to cm
|
||||
constexpr float RADAR_MIN_VALID_DIST = 30.0f; // Minimum valid distance in cm
|
||||
|
||||
int16_t Radar::decodeSignMag(uint16_t raw) {
|
||||
int16_t magnitude = raw & 0x7FFF;
|
||||
return (raw & 0x8000) ? magnitude : -magnitude;
|
||||
}
|
||||
|
||||
void Radar::init() {
|
||||
Serial2.begin(RADAR_BAUD, SERIAL_8N1, SensorPins::RADAR_RX, SensorPins::RADAR_TX);
|
||||
}
|
||||
|
||||
bool Radar::update() {
|
||||
bool newData = false;
|
||||
|
||||
while (Serial2.available()) {
|
||||
uint8_t b = Serial2.read();
|
||||
|
||||
if (!inFrame) {
|
||||
// Looking for header
|
||||
if (b == RADAR_HEADER[headerMatch]) {
|
||||
rxBuf[headerMatch] = b;
|
||||
headerMatch++;
|
||||
if (headerMatch == 4) {
|
||||
inFrame = true;
|
||||
bufIdx = 4;
|
||||
headerMatch = 0;
|
||||
}
|
||||
} else if (b == RADAR_HEADER[0]) {
|
||||
headerMatch = 1;
|
||||
rxBuf[0] = b;
|
||||
} else {
|
||||
headerMatch = 0;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
// In frame - collect bytes
|
||||
if (bufIdx < sizeof(rxBuf)) {
|
||||
rxBuf[bufIdx++] = b;
|
||||
}
|
||||
|
||||
// Check for footer
|
||||
if (bufIdx >= 6 && rxBuf[bufIdx - 2] == RADAR_FOOTER[0] && rxBuf[bufIdx - 1] == RADAR_FOOTER[1]) {
|
||||
parseFrame();
|
||||
newData = true;
|
||||
inFrame = false;
|
||||
bufIdx = 0;
|
||||
}
|
||||
|
||||
// Overflow protection
|
||||
if (bufIdx >= sizeof(rxBuf)) {
|
||||
inFrame = false;
|
||||
bufIdx = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return newData;
|
||||
}
|
||||
|
||||
void Radar::parseFrame() {
|
||||
for (int i = 0; i < RADAR_MAX_TARGETS; i++) {
|
||||
int offset = 4 + (i * 6);
|
||||
|
||||
uint16_t x_raw = rxBuf[offset] | (rxBuf[offset + 1] << 8);
|
||||
uint16_t y_raw = rxBuf[offset + 2] | (rxBuf[offset + 3] << 8);
|
||||
uint16_t spd_raw = rxBuf[offset + 4] | (rxBuf[offset + 5] << 8);
|
||||
|
||||
targets[i].x = decodeSignMag(x_raw) * RADAR_DISTANCE_SCALE;
|
||||
targets[i].y = (int16_t)(y_raw - 0x8000) * RADAR_DISTANCE_SCALE;
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const RadarTarget& Radar::getTarget(uint8_t index) const {
|
||||
if (index >= RADAR_MAX_TARGETS) index = 0;
|
||||
return targets[index];
|
||||
}
|
||||
|
||||
uint8_t Radar::getTargetCount() const {
|
||||
uint8_t count = 0;
|
||||
for (int i = 0; i < RADAR_MAX_TARGETS; i++) {
|
||||
if (targets[i].valid) count++;
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
uint16_t Radar::packPayload(uint8_t* buffer) const {
|
||||
// Format: count(1) + [valid(1), x(2), y(2), speed(2), angle(2)] * 3 = 28 bytes
|
||||
buffer[0] = getTargetCount();
|
||||
uint16_t offset = 1;
|
||||
|
||||
for (int i = 0; i < RADAR_MAX_TARGETS; i++) {
|
||||
buffer[offset++] = targets[i].valid ? 1 : 0;
|
||||
|
||||
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;
|
||||
buffer[offset++] = y & 0xFF;
|
||||
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;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// ADXL345 Implementation
|
||||
// ============================================================================
|
||||
|
||||
ADXL345::ADXL345(uint8_t addr) : addr(addr) {}
|
||||
|
||||
bool ADXL345::init() {
|
||||
Wire.begin(SensorPins::IMU_SDA, SensorPins::IMU_SCL);
|
||||
delay(100);
|
||||
|
||||
uint8_t id = read8(0x00); // DEVID register (should be 0xE5)
|
||||
if (id != 0xE5) {
|
||||
ready = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Enable measurement mode
|
||||
write8(0x2D, 0x08); // POWER_CTL: measure mode
|
||||
|
||||
ready = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ADXL345::update() {
|
||||
if (!ready) return false;
|
||||
|
||||
readAccelData();
|
||||
return true;
|
||||
}
|
||||
|
||||
float ADXL345::getPitch() const {
|
||||
// Approximate pitch from Y/Z acceleration (Y = front/back axis)
|
||||
// This is not as accurate as a proper IMU with gyroscope
|
||||
if (accelZ == 0) return 0;
|
||||
return atan2(-accelY, sqrt(accelX * accelX + accelZ * accelZ)) * 180.0f / PI;
|
||||
}
|
||||
|
||||
float ADXL345::getRoll() const {
|
||||
// Approximate roll from X/Z acceleration (X = left/right axis)
|
||||
if (accelZ == 0) return 0;
|
||||
return atan2(accelX, accelZ) * 180.0f / PI;
|
||||
}
|
||||
|
||||
void ADXL345::getEulerAngles(float& pitch, float& roll) const {
|
||||
// Calculate Euler angles from accelerometer data
|
||||
// Note: Heading (yaw) cannot be determined from accelerometer alone
|
||||
// Coordinate system: X=left/right, Y=front/back, Z=up/down
|
||||
|
||||
if (!ready) {
|
||||
pitch = 0.0f;
|
||||
roll = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
// Pitch (front/back tilt) = atan2(-accelY, sqrt(accelX² + accelZ²))
|
||||
// Roll (left/right tilt) = atan2(accelX, accelZ)
|
||||
|
||||
float accelMagnitude = sqrt(accelX * accelX + accelZ * accelZ);
|
||||
if (accelMagnitude > 0.01f) { // Avoid division by very small numbers
|
||||
pitch = atan2(-accelY, accelMagnitude) * 180.0f / PI;
|
||||
} else {
|
||||
pitch = 0.0f;
|
||||
}
|
||||
|
||||
if (fabs(accelZ) > 0.01f) { // Avoid division by zero
|
||||
roll = atan2(accelX, accelZ) * 180.0f / PI;
|
||||
} else {
|
||||
roll = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t ADXL345::packPayload(uint8_t* buffer) const {
|
||||
// Format: accelX(2) + accelY(2) + accelZ(2) + pitch(2) + roll(2), all ×100
|
||||
// Accelerations in g-forces ×100, angles in degrees ×100
|
||||
|
||||
// Pack acceleration data
|
||||
int16_t x = (int16_t)(accelX * 100.0f);
|
||||
int16_t y = (int16_t)(accelY * 100.0f);
|
||||
int16_t z = (int16_t)(accelZ * 100.0f);
|
||||
|
||||
buffer[0] = x & 0xFF;
|
||||
buffer[1] = (x >> 8) & 0xFF;
|
||||
buffer[2] = y & 0xFF;
|
||||
buffer[3] = (y >> 8) & 0xFF;
|
||||
buffer[4] = z & 0xFF;
|
||||
buffer[5] = (z >> 8) & 0xFF;
|
||||
|
||||
// Calculate and pack Euler angles
|
||||
float pitch_deg, roll_deg;
|
||||
getEulerAngles(pitch_deg, roll_deg);
|
||||
|
||||
int16_t pitch = (int16_t)(pitch_deg * 100.0f);
|
||||
int16_t roll = (int16_t)(roll_deg * 100.0f);
|
||||
|
||||
buffer[6] = pitch & 0xFF;
|
||||
buffer[7] = (pitch >> 8) & 0xFF;
|
||||
buffer[8] = roll & 0xFF;
|
||||
buffer[9] = (roll >> 8) & 0xFF;
|
||||
|
||||
return 10;
|
||||
}
|
||||
|
||||
void ADXL345::write8(uint8_t reg, uint8_t value) {
|
||||
Wire.beginTransmission(addr);
|
||||
Wire.write(reg);
|
||||
Wire.write(value);
|
||||
Wire.endTransmission();
|
||||
}
|
||||
|
||||
uint8_t ADXL345::read8(uint8_t reg) {
|
||||
Wire.beginTransmission(addr);
|
||||
Wire.write(reg);
|
||||
Wire.endTransmission();
|
||||
Wire.requestFrom(addr, (uint8_t)1);
|
||||
return Wire.available() ? Wire.read() : 0xFF;
|
||||
}
|
||||
|
||||
void ADXL345::readAccelData() {
|
||||
// Read 6 bytes starting from DATAX0 register (0x32)
|
||||
Wire.beginTransmission(addr);
|
||||
Wire.write(0x32); // Start at DATAX0
|
||||
Wire.endTransmission();
|
||||
|
||||
Wire.requestFrom(addr, (uint8_t)6);
|
||||
if (Wire.available() < 6) return;
|
||||
|
||||
// ADXL345 outputs 10-bit values (2 bytes per axis)
|
||||
int16_t x_raw = Wire.read() | (Wire.read() << 8);
|
||||
int16_t y_raw = Wire.read() | (Wire.read() << 8);
|
||||
int16_t z_raw = Wire.read() | (Wire.read() << 8);
|
||||
|
||||
// Convert to g-forces: ±2g range, 10-bit = ±512 LSB, 4mg/LSB
|
||||
const float scale = 0.00390625f; // 4mg/LSB = 0.004g, but we use 1/256 for 10-bit
|
||||
|
||||
accelX = x_raw * scale;
|
||||
accelY = y_raw * scale;
|
||||
accelZ = z_raw * scale;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Sensor Manager Implementation
|
||||
// ============================================================================
|
||||
|
||||
void SensorManager::init() {
|
||||
radar.init();
|
||||
|
||||
if (adxl.init()) {
|
||||
Serial.println("[Sensors] ADXL345 initialized");
|
||||
} else {
|
||||
Serial.println("[Sensors] ADXL345 not detected");
|
||||
}
|
||||
|
||||
Serial.println("[Sensors] Radar initialized");
|
||||
}
|
||||
|
||||
void SensorManager::update() {
|
||||
// Update sensors
|
||||
radar.update();
|
||||
if (adxl.isReady()) {
|
||||
adxl.update();
|
||||
}
|
||||
|
||||
// Handle streaming
|
||||
unsigned long now = millis();
|
||||
|
||||
if (adxlStreamEnabled && adxl.isReady() && (now - lastADXLSend >= adxlInterval)) {
|
||||
sendADXLPacket();
|
||||
lastADXLSend = now;
|
||||
}
|
||||
|
||||
if (radarStreamEnabled && (now - lastRadarSend >= radarInterval)) {
|
||||
sendRadarPacket();
|
||||
lastRadarSend = now;
|
||||
}
|
||||
}
|
||||
|
||||
void SensorManager::enableADXLStream(bool enable, uint16_t intervalMs) {
|
||||
adxlStreamEnabled = enable;
|
||||
adxlInterval = intervalMs;
|
||||
lastADXLSend = millis();
|
||||
}
|
||||
|
||||
void SensorManager::enableRadarStream(bool enable, uint16_t intervalMs) {
|
||||
radarStreamEnabled = enable;
|
||||
radarInterval = intervalMs;
|
||||
lastRadarSend = millis();
|
||||
}
|
||||
|
||||
void SensorManager::sendADXLPacket() {
|
||||
uint8_t payload[32]; // Buffer sized for current/future payload expansion
|
||||
uint16_t len = adxl.packPayload(payload);
|
||||
sendPacket(Tag::IMU, payload, len); // Reuse IMU tag for compatibility
|
||||
}
|
||||
|
||||
void SensorManager::sendRadarPacket() {
|
||||
uint8_t payload[32]; // Updated size for angle field
|
||||
uint16_t len = radar.packPayload(payload);
|
||||
sendPacket(Tag::RADAR, payload, len);
|
||||
}
|
||||
|
||||
130
sensors.h
130
sensors.h
|
|
@ -1,130 +0,0 @@
|
|||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include <Wire.h>
|
||||
|
||||
// ============================================================================
|
||||
// Pin Configuration
|
||||
// ============================================================================
|
||||
|
||||
namespace SensorPins {
|
||||
// Radar (RD-03D) on Serial2
|
||||
constexpr int RADAR_RX = 4;
|
||||
constexpr int RADAR_TX = 5;
|
||||
|
||||
// IMU (BNO055) on I2C
|
||||
constexpr int IMU_SDA = 8;
|
||||
constexpr int IMU_SCL = 9;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Radar - RD-03D mmWave Sensor
|
||||
// ============================================================================
|
||||
|
||||
constexpr int RADAR_MAX_TARGETS = 3;
|
||||
constexpr uint32_t RADAR_BAUD = 256000;
|
||||
|
||||
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
|
||||
};
|
||||
|
||||
class Radar {
|
||||
public:
|
||||
void init();
|
||||
bool update(); // Returns true if new data parsed
|
||||
|
||||
const RadarTarget& getTarget(uint8_t index) const;
|
||||
uint8_t getTargetCount() const;
|
||||
|
||||
// Pack all targets into a payload buffer, returns length
|
||||
uint16_t packPayload(uint8_t* buffer) const;
|
||||
|
||||
private:
|
||||
RadarTarget targets[RADAR_MAX_TARGETS] = {};
|
||||
uint8_t rxBuf[64];
|
||||
uint8_t bufIdx = 0;
|
||||
uint8_t headerMatch = 0;
|
||||
bool inFrame = false;
|
||||
|
||||
void parseFrame();
|
||||
static int16_t decodeSignMag(uint16_t raw);
|
||||
};
|
||||
|
||||
// ============================================================================
|
||||
// IMU - ADXL345 3-axis Accelerometer
|
||||
// ============================================================================
|
||||
|
||||
class ADXL345 {
|
||||
public:
|
||||
ADXL345(uint8_t addr = 0x53);
|
||||
|
||||
bool init();
|
||||
bool update(); // Read latest values
|
||||
|
||||
// Get acceleration in g-forces
|
||||
float getAccelX() const { return accelX; }
|
||||
float getAccelY() const { return accelY; }
|
||||
float getAccelZ() const { return accelZ; }
|
||||
|
||||
// Get tilt angles in degrees (approximated from gravity)
|
||||
float getPitch() const; // Pitch from Y/Z acceleration (Y=front/back)
|
||||
float getRoll() const; // Roll from X/Z acceleration (X=left/right)
|
||||
|
||||
// Get Euler angles (pitch and roll only, no heading without magnetometer)
|
||||
void getEulerAngles(float& pitch, float& roll) const;
|
||||
|
||||
bool isReady() const { return ready; }
|
||||
|
||||
// Pack into payload buffer (10 bytes: x,y,z accel + pitch,roll angles)
|
||||
uint16_t packPayload(uint8_t* buffer) const;
|
||||
|
||||
private:
|
||||
uint8_t addr;
|
||||
bool ready = false;
|
||||
float accelX = 0, accelY = 0, accelZ = 0;
|
||||
|
||||
void write8(uint8_t reg, uint8_t value);
|
||||
uint8_t read8(uint8_t reg);
|
||||
void readAccelData();
|
||||
};
|
||||
|
||||
// ============================================================================
|
||||
// Global Instances
|
||||
// ============================================================================
|
||||
|
||||
extern Radar radar;
|
||||
extern ADXL345 adxl;
|
||||
|
||||
// ============================================================================
|
||||
// Sensor Manager
|
||||
// ============================================================================
|
||||
|
||||
class SensorManager {
|
||||
public:
|
||||
void init();
|
||||
void update();
|
||||
|
||||
// Streaming control
|
||||
void enableADXLStream(bool enable, uint16_t intervalMs = 100);
|
||||
void enableRadarStream(bool enable, uint16_t intervalMs = 100);
|
||||
|
||||
bool isADXLStreamEnabled() const { return adxlStreamEnabled; }
|
||||
bool isRadarStreamEnabled() const { return radarStreamEnabled; }
|
||||
|
||||
private:
|
||||
bool adxlStreamEnabled = true;
|
||||
bool radarStreamEnabled = true;
|
||||
uint16_t adxlInterval = 500; // 500ms = 2 packets per second
|
||||
uint16_t radarInterval = 10;
|
||||
unsigned long lastADXLSend = 0;
|
||||
unsigned long lastRadarSend = 0;
|
||||
|
||||
void sendADXLPacket();
|
||||
void sendRadarPacket();
|
||||
};
|
||||
|
||||
extern SensorManager sensors;
|
||||
|
||||
Loading…
Reference in New Issue