Major changes to bring in line with protocol v2
parent
bcb5f25579
commit
bc6452c256
1441
HansonServo.ino
1441
HansonServo.ino
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,404 @@
|
|||
# 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_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 (heading, roll, pitch) |
|
||||
| `RDAR` | Radar target data |
|
||||
| `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]
|
||||
```
|
||||
**Response:** `ACK!` on success, `NACK` if file not found
|
||||
|
||||
---
|
||||
|
||||
### 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:**
|
||||
```
|
||||
[heading: 2 bytes LE, signed] // degrees × 100
|
||||
[roll: 2 bytes LE, signed] // degrees × 100
|
||||
[pitch: 2 bytes LE, signed] // degrees × 100
|
||||
```
|
||||
*Sent automatically when IMU streaming is enabled*
|
||||
|
||||
#### `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*
|
||||
|
||||
---
|
||||
|
||||
### 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
|
||||
|
||||
|
|
@ -0,0 +1,555 @@
|
|||
#include "commands.h"
|
||||
#include "nodegraph.h"
|
||||
#include "sensors.h"
|
||||
#include "esp_system.h"
|
||||
#include "soc/rtc_cntl_reg.h"
|
||||
#include <vector>
|
||||
|
||||
// External references
|
||||
extern RobotConfig config;
|
||||
extern ServoManager servoManager;
|
||||
|
||||
// Global state instances
|
||||
AnimationState animState;
|
||||
MotorStreamState motorStream;
|
||||
|
||||
// ============================================================================
|
||||
// AnimationState
|
||||
// ============================================================================
|
||||
|
||||
void AnimationState::play(PlayMode mode, uint8_t repeats) {
|
||||
current = &animation;
|
||||
current->setActive(true);
|
||||
playMode = mode;
|
||||
repeatsRemaining = repeats;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
// 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);
|
||||
}
|
||||
// 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();
|
||||
if (fileSize > MAX_PAYLOAD_SIZE) {
|
||||
sendNack(Tag::FLOAD, "File too large");
|
||||
file.close();
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t* buffer = (uint8_t*)malloc(fileSize);
|
||||
if (!buffer) {
|
||||
sendNack(Tag::FLOAD, "Out of memory");
|
||||
file.close();
|
||||
return;
|
||||
}
|
||||
|
||||
file.read(buffer, fileSize);
|
||||
file.close();
|
||||
|
||||
sendPacket(Tag::FLOAD, buffer, fileSize);
|
||||
free(buffer);
|
||||
}
|
||||
|
||||
void handleFileSave(const uint8_t* payload, uint16_t len) {
|
||||
if (parseAndSaveAnimation(payload, len, animState.animation)) {
|
||||
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 < 4) {
|
||||
sendNack(Tag::FPLAY, "Invalid request");
|
||||
return;
|
||||
}
|
||||
|
||||
uint16_t filenameLen = payload[0] | (payload[1] << 8);
|
||||
if (len < 2 + filenameLen + 2) {
|
||||
sendNack(Tag::FPLAY, "Invalid filename");
|
||||
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];
|
||||
|
||||
animState.animation.clear();
|
||||
String fullPath = "/" + String(filename);
|
||||
|
||||
if (animState.animation.loadFromFile(fullPath.c_str())) {
|
||||
animState.play(mode, repeats);
|
||||
sendAck(Tag::FPLAY);
|
||||
sendMessage("Playing: " + String(filename));
|
||||
} else {
|
||||
sendNack(Tag::FPLAY, "Load failed");
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// 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) {
|
||||
if (len < 2) return false;
|
||||
|
||||
uint16_t filenameLen = payload[0] | (payload[1] << 8);
|
||||
if (len < 2 + filenameLen + 18) return false;
|
||||
|
||||
char filename[128];
|
||||
memcpy(filename, payload + 2, min((uint16_t)127, filenameLen));
|
||||
filename[min((uint16_t)127, filenameLen)] = '\0';
|
||||
|
||||
const uint8_t* ptr = payload + 2 + filenameLen;
|
||||
uint16_t remaining = len - 2 - filenameLen;
|
||||
|
||||
sendMessage("Saving: " + String(filename));
|
||||
|
||||
// Parse header
|
||||
if (remaining < 18) 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);
|
||||
|
||||
uint16_t curveCount = ptr[16] | (ptr[17] << 8);
|
||||
ptr += 18;
|
||||
remaining -= 18;
|
||||
|
||||
// Parse curves
|
||||
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;
|
||||
|
||||
// Parse node graph
|
||||
if (remaining > 0) {
|
||||
loadNodeGraph(ptr, remaining, animation.nodeGraph);
|
||||
animation.nodeGraph.bindAnimationContext(&animation);
|
||||
}
|
||||
|
||||
// Save to file
|
||||
String fullPath = "/" + String(filename);
|
||||
animation.saveToFile(fullPath.c_str());
|
||||
|
||||
sendMessage("Saved: " + String(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);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,83 @@
|
|||
#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;
|
||||
|
||||
void play(PlayMode mode, uint8_t repeats = 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);
|
||||
|
||||
// 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);
|
||||
|
||||
// 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);
|
||||
|
|
@ -0,0 +1,60 @@
|
|||
#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
|
||||
);
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,60 @@
|
|||
#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;
|
||||
|
||||
|
|
@ -2,9 +2,12 @@
|
|||
#include "animation.h"
|
||||
#include "noise.h"
|
||||
#include "RobotConfig.h"
|
||||
#include "sensors.h"
|
||||
#include <cstring>
|
||||
|
||||
extern RobotConfig config;
|
||||
extern IMU imu;
|
||||
extern Radar radar;
|
||||
|
||||
|
||||
// CurveNode evaluation
|
||||
|
|
@ -61,6 +64,60 @@ void VariableNode::evaluate(uint32_t currentTick) {
|
|||
case VAR_SERVO_FEEDBACK:
|
||||
outputValue = config.getMotorPosition(arg0);
|
||||
break;
|
||||
|
||||
// IMU sources - scale to 0-4095 range
|
||||
case VAR_IMU_HEADING:
|
||||
// Heading: 0-360° → 0-4095
|
||||
if (imu.isReady()) {
|
||||
outputValue = (uint16_t)((imu.getHeading() / 360.0f) * 4095.0f);
|
||||
} else {
|
||||
outputValue = 2048; // Center if not ready
|
||||
}
|
||||
break;
|
||||
case VAR_IMU_PITCH:
|
||||
// Pitch: -90° to +90° → 0-4095 (2048 = level)
|
||||
if (imu.isReady()) {
|
||||
float pitch = constrain(imu.getPitch(), -90.0f, 90.0f);
|
||||
outputValue = (uint16_t)(((pitch + 90.0f) / 180.0f) * 4095.0f);
|
||||
} else {
|
||||
outputValue = 2048;
|
||||
}
|
||||
break;
|
||||
case VAR_IMU_ROLL:
|
||||
// Roll: -180° to +180° → 0-4095 (2048 = level)
|
||||
if (imu.isReady()) {
|
||||
float roll = constrain(imu.getRoll(), -180.0f, 180.0f);
|
||||
outputValue = (uint16_t)(((roll + 180.0f) / 360.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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -19,7 +19,14 @@ enum VariableSource {
|
|||
VAR_FACE_Y,
|
||||
VAR_SINE,
|
||||
VAR_ANALOG,
|
||||
VAR_SERVO_FEEDBACK
|
||||
VAR_SERVO_FEEDBACK,
|
||||
// IMU sources (BNO055)
|
||||
VAR_IMU_HEADING, // 0-360 degrees → 0-4095
|
||||
VAR_IMU_PITCH, // -90 to +90 degrees → 0-4095 (2048 = level)
|
||||
VAR_IMU_ROLL, // -180 to +180 degrees → 0-4095 (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)
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,209 @@
|
|||
#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;
|
||||
}
|
||||
|
|
@ -0,0 +1,118 @@
|
|||
#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
|
||||
|
||||
// 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'}; // IMU data (heading, roll, pitch)
|
||||
constexpr char RADAR[4] = {'R','D','A','R'}; // Radar targets
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
|
@ -0,0 +1,271 @@
|
|||
#include "sensors.h"
|
||||
#include "protocol.h"
|
||||
|
||||
// ============================================================================
|
||||
// Global Instances
|
||||
// ============================================================================
|
||||
|
||||
Radar radar;
|
||||
IMU imu;
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
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)] * 3 = 22 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);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
return offset;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// IMU Implementation
|
||||
// ============================================================================
|
||||
|
||||
IMU::IMU(uint8_t addr) : addr(addr) {}
|
||||
|
||||
bool IMU::init() {
|
||||
Wire.begin(SensorPins::IMU_SDA, SensorPins::IMU_SCL);
|
||||
delay(100);
|
||||
|
||||
uint8_t id = read8(0x00); // CHIP_ID register
|
||||
if (id != 0xA0) {
|
||||
ready = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Enter config mode
|
||||
write8(0x3D, 0x00);
|
||||
delay(25);
|
||||
|
||||
// Set NDOF fusion mode
|
||||
write8(0x3D, 0x0C);
|
||||
delay(25);
|
||||
|
||||
ready = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool IMU::update() {
|
||||
if (!ready) return false;
|
||||
|
||||
Wire.beginTransmission(addr);
|
||||
Wire.write(0x1A); // Euler angles start register
|
||||
Wire.endTransmission();
|
||||
|
||||
Wire.requestFrom(addr, (uint8_t)6);
|
||||
if (Wire.available() < 6) return false;
|
||||
|
||||
int16_t rawHeading = Wire.read() | (Wire.read() << 8);
|
||||
int16_t rawPitch = Wire.read() | (Wire.read() << 8);
|
||||
int16_t rawRoll = Wire.read() | (Wire.read() << 8);
|
||||
|
||||
// Convert from 1/16 degree units
|
||||
heading = rawHeading / 16.0f;
|
||||
roll = -(rawRoll / 16.0f); // Inverted so right roll is positive
|
||||
pitch = rawPitch / 16.0f;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
uint16_t IMU::packPayload(uint8_t* buffer) const {
|
||||
// Format: heading(2) + roll(2) + pitch(2) as int16 * 100
|
||||
int16_t h = (int16_t)(heading * 100.0f);
|
||||
int16_t r = (int16_t)(roll * 100.0f);
|
||||
int16_t p = (int16_t)(pitch * 100.0f);
|
||||
|
||||
buffer[0] = h & 0xFF;
|
||||
buffer[1] = (h >> 8) & 0xFF;
|
||||
buffer[2] = r & 0xFF;
|
||||
buffer[3] = (r >> 8) & 0xFF;
|
||||
buffer[4] = p & 0xFF;
|
||||
buffer[5] = (p >> 8) & 0xFF;
|
||||
|
||||
return 6;
|
||||
}
|
||||
|
||||
void IMU::write8(uint8_t reg, uint8_t value) {
|
||||
Wire.beginTransmission(addr);
|
||||
Wire.write(reg);
|
||||
Wire.write(value);
|
||||
Wire.endTransmission();
|
||||
}
|
||||
|
||||
uint8_t IMU::read8(uint8_t reg) {
|
||||
Wire.beginTransmission(addr);
|
||||
Wire.write(reg);
|
||||
Wire.endTransmission();
|
||||
Wire.requestFrom(addr, (uint8_t)1);
|
||||
return Wire.available() ? Wire.read() : 0xFF;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Sensor Manager Implementation
|
||||
// ============================================================================
|
||||
|
||||
void SensorManager::init() {
|
||||
radar.init();
|
||||
|
||||
if (imu.init()) {
|
||||
Serial.println("[Sensors] IMU initialized");
|
||||
} else {
|
||||
Serial.println("[Sensors] IMU not detected");
|
||||
}
|
||||
|
||||
Serial.println("[Sensors] Radar initialized");
|
||||
}
|
||||
|
||||
void SensorManager::update() {
|
||||
// Update sensors
|
||||
radar.update();
|
||||
if (imu.isReady()) {
|
||||
imu.update();
|
||||
}
|
||||
|
||||
// Handle streaming
|
||||
unsigned long now = millis();
|
||||
|
||||
if (imuStreamEnabled && imu.isReady() && (now - lastIMUSend >= imuInterval)) {
|
||||
sendIMUPacket();
|
||||
lastIMUSend = now;
|
||||
}
|
||||
|
||||
if (radarStreamEnabled && (now - lastRadarSend >= radarInterval)) {
|
||||
sendRadarPacket();
|
||||
lastRadarSend = now;
|
||||
}
|
||||
}
|
||||
|
||||
void SensorManager::enableIMUStream(bool enable, uint16_t intervalMs) {
|
||||
imuStreamEnabled = enable;
|
||||
imuInterval = intervalMs;
|
||||
lastIMUSend = millis();
|
||||
}
|
||||
|
||||
void SensorManager::enableRadarStream(bool enable, uint16_t intervalMs) {
|
||||
radarStreamEnabled = enable;
|
||||
radarInterval = intervalMs;
|
||||
lastRadarSend = millis();
|
||||
}
|
||||
|
||||
void SensorManager::sendIMUPacket() {
|
||||
uint8_t payload[6];
|
||||
uint16_t len = imu.packPayload(payload);
|
||||
sendPacket(Tag::IMU, payload, len);
|
||||
}
|
||||
|
||||
void SensorManager::sendRadarPacket() {
|
||||
uint8_t payload[32];
|
||||
uint16_t len = radar.packPayload(payload);
|
||||
sendPacket(Tag::RADAR, payload, len);
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,121 @@
|
|||
#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
|
||||
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 - BNO055 9-DOF Sensor
|
||||
// ============================================================================
|
||||
|
||||
class IMU {
|
||||
public:
|
||||
IMU(uint8_t addr = 0x29);
|
||||
|
||||
bool init();
|
||||
bool update(); // Read latest values
|
||||
|
||||
// Get euler angles in degrees
|
||||
float getHeading() const { return heading; }
|
||||
float getRoll() const { return roll; }
|
||||
float getPitch() const { return pitch; }
|
||||
|
||||
bool isReady() const { return ready; }
|
||||
|
||||
// Pack into payload buffer (6 bytes: h, r, p as int16 * 100)
|
||||
uint16_t packPayload(uint8_t* buffer) const;
|
||||
|
||||
private:
|
||||
uint8_t addr;
|
||||
bool ready = false;
|
||||
float heading = 0, roll = 0, pitch = 0;
|
||||
|
||||
void write8(uint8_t reg, uint8_t value);
|
||||
uint8_t read8(uint8_t reg);
|
||||
};
|
||||
|
||||
// ============================================================================
|
||||
// Global Instances
|
||||
// ============================================================================
|
||||
|
||||
extern Radar radar;
|
||||
extern IMU imu;
|
||||
|
||||
// ============================================================================
|
||||
// Sensor Manager
|
||||
// ============================================================================
|
||||
|
||||
class SensorManager {
|
||||
public:
|
||||
void init();
|
||||
void update();
|
||||
|
||||
// Streaming control
|
||||
void enableIMUStream(bool enable, uint16_t intervalMs = 100);
|
||||
void enableRadarStream(bool enable, uint16_t intervalMs = 100);
|
||||
|
||||
bool isIMUStreamEnabled() const { return imuStreamEnabled; }
|
||||
bool isRadarStreamEnabled() const { return radarStreamEnabled; }
|
||||
|
||||
private:
|
||||
bool imuStreamEnabled = false;
|
||||
bool radarStreamEnabled = false;
|
||||
uint16_t imuInterval = 100;
|
||||
uint16_t radarInterval = 100;
|
||||
unsigned long lastIMUSend = 0;
|
||||
unsigned long lastRadarSend = 0;
|
||||
|
||||
void sendIMUPacket();
|
||||
void sendRadarPacket();
|
||||
};
|
||||
|
||||
extern SensorManager sensors;
|
||||
|
||||
Loading…
Reference in New Issue