From bc6452c2567a02f6acf577c9650b847018e7347f Mon Sep 17 00:00:00 2001 From: Jake Date: Wed, 10 Dec 2025 14:25:04 +0800 Subject: [PATCH] Major changes to bring in line with protocol v2 --- HansonServo.ino | 1453 ++++++++--------------------------------- PROTOCOL_MIGRATION.md | 404 ++++++++++++ commands.cpp | 555 ++++++++++++++++ commands.h | 83 +++ motorcontrol.cpp | 60 ++ motorcontrol.h | 60 ++ nodegraph.cpp | 57 ++ nodegraph.h | 9 +- protocol.cpp | 209 ++++++ protocol.h | 118 ++++ sensors.cpp | 271 ++++++++ sensors.h | 121 ++++ 12 files changed, 2218 insertions(+), 1182 deletions(-) create mode 100644 PROTOCOL_MIGRATION.md create mode 100644 commands.cpp create mode 100644 commands.h create mode 100644 motorcontrol.cpp create mode 100644 motorcontrol.h create mode 100644 protocol.cpp create mode 100644 protocol.h create mode 100644 sensors.cpp create mode 100644 sensors.h diff --git a/HansonServo.ino b/HansonServo.ino index bb2de69..dd36388 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -1,1059 +1,89 @@ +/** + * HansonServo Firmware + * + * Unified robot controller with: + * - Feetech servo control (SCS/STS) + * - Animation playback via node graphs + * - BNO055 IMU + * - RD-03D mmWave radar + * - CRC16 tagged packet protocol + * + * Protocol: 0xA5 0x5A [TAG 4B][LEN 2B][SEQ 2B][PAYLOAD][CRC16 2B] + */ -#include -#include "feetech.h" +#include +#include "protocol.h" +#include "commands.h" +#include "motorcontrol.h" #include "animation.h" #include "nodegraph.h" #include "RobotConfig.h" +#include "sensors.h" + +// ============================================================================ +// Global State +// ============================================================================ RobotConfig config; -#define DEVICE_NAME "Little Sophia" -#define FIRMWARE_VERSION "0.0.1" +// 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; -#define HEADER1 0xAA -#define HEADER2 0x55 -#define MAX_PAYLOAD_SIZE 6000 // 10 KB -uint8_t payload[MAX_PAYLOAD_SIZE]; // Global or static - -#define MODE_NORMAL 0 -#define MODE_FEETECH 1 -uint8_t mode = MODE_NORMAL; - -#define CMD_ID_REQUEST 0x01 -#define CMD_FILE_LIST 0x02 -#define CMD_LOAD_FILE 0x03 -#define CMD_DELETE_FILE 0x04 -#define CMD_SAVE_FILE 0x05 -#define CMD_MESSAGE 0x06 -#define CMD_SET_POSITION 0x07 -#define CMD_PLAY_FILE 0x08 -#define CMD_SCAN_CHANNEL 0x09 -#define CMD_WRITE_DATA 0x10 -#define CMD_WRITE_CONFIG_UPDATE 0x12 -#define CMD_START_POSITION_STREAM 0x14 -#define POSITION_STREAM 0x15 - - -// ESP32 S2 PINOUT -#define CH0_RX_PIN 13 -#define CH0_TX_PIN 12 - -// #define RX_PIN 18 // RO -// #define TX_PIN 17 // DI -#define CH1_RX_PIN 11 // RO -#define CH1_TX_PIN 10 // DI -#define DE_PIN 7 // Driver Enable -#define RE_PIN 8 // Receiver Enable - -#define PLAY_IDLE 0x00 -#define PLAY_ONCE 0x01 -#define PLAY_LOOP 0x02 -#define PLAY_REPEAT 0x03 -uint8_t playMode = PLAY_IDLE; -uint8_t repeatsRemaining = 0; - -Animation anim; -Animation* currentAnimation; - -bool streamPositions = false; -unsigned long lastStreamPositions = 0; - -Feetech* servos[2]; - -uint16_t flipBytes(uint16_t value) { - return (value >> 8) | (value << 8); -} - - -uint8_t ids[NUM_CHANNELS] = { 10, 11, 12, 13, 14 }; -uint16_t pos1[] = { 0, 0, 0, 0, 0 }; -uint16_t pos2[] = { 1023, 1023, 1023, 1023, 1023 }; - -uint8_t idsSTS[NUM_CHANNELS] = { 15, 103 }; -uint16_t pos1STS[] = { 0, 0 }; -uint16_t pos2STS[] = { 1023, 1023 }; - -#define WAVE_PERIOD_CS 400 // 4 seconds = 400 centiseconds -#define WAVE_MAX 4095 +// ============================================================================ +// 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); // ranges from -1 to +1 - float scaled = (sine + 1.0) * (WAVE_MAX / 2.0); // scale to 0–4095 + float sine = sin(theta); + float scaled = (sine + 1.0) * (WAVE_MAX / 2.0); return (uint16_t)round(scaled); } +// ============================================================================ +// Protocol Handler +// ============================================================================ -void setup() { - Serial.setRxBufferSize(8192); // Must be set BEFORE Serial.begin() - Serial.begin(1000000); - //Serial1.begin(1000000, SERIAL_8N1, CH0_RX_PIN, CH0_TX_PIN) - for (int i = 0; i < 5; i++) { - Serial.println(i); - delay(500); - } - - pinMode(6, INPUT); - pinMode(7, INPUT); - - servos[0] = new Feetech(Serial1, DE_PIN, RE_PIN, CH0_TX_PIN, CH0_RX_PIN); // SCS - servos[0]->setFeetechMode(Feetech::MODE_SCS); - servos[1] = new Feetech(Serial2, DE_PIN, RE_PIN, CH1_TX_PIN, CH1_RX_PIN); // STS - servos[1]->setFeetechMode(Feetech::MODE_SCS); - - servos[0]->begin(); - // servos[1]->begin(); - - if (!FFat.begin(true)) { - Serial.println("FFat mount failed"); - return; - } else { - Serial.println("FFat mount successful"); - } - - if (config.loadOrCreateDefault()) { - Serial.println("Robot config ready."); - } else { - Serial.println("Failed to initialize robot config."); - } - - // Print serialized config - Serial.println(config.serializeJSON()); - std::vector bytes = config.serializeToBytes(); - String output = ""; - - for (size_t i = 0; i < bytes.size(); ++i) { - if (bytes[i] < 16) output += "0"; // pad single-digit hex - output += String(bytes[i], HEX); - if (i < bytes.size() - 1) output += " "; - } - Serial.println(output); - - - // PLAY ANIMATION - // anim.loadFromFile("/animation.anim"); - // currentAnimation = &anim; - // currentAnimation->setActive(true); - // playMode = PLAY_ONCE; - //Serial.println(anim.header.frameCount); - //Serial.println(anim.printCurves()); - // Serial.println("Model"); - // Serial.println(servos[0]->getModel(10)); - // delay(20); - // Serial.println(servos[0]->getModel(11)); - // delay(50); - // Serial.println(servos[0]->getModel(12)); - // Serial.println(servos[0]->getModel(17)); - // Serial.println(servos[0]->getModel(14)); -} - -void SetID(uint8_t oldID, uint8_t newID) { - Serial.println("Setting Lock to 0"); - Serial.println(servos[1]->setLock(oldID, 0)); - delay(1000); - Serial.print("Changing ID "); - Serial.print(oldID); - Serial.print(" to "); - Serial.println(newID); - Serial.println(servos[1]->setID(oldID, newID)); - delay(1000); - Serial.println("Setting Lock to 1"); - Serial.println(servos[1]->setLock(newID, 1)); - delay(1000); -} - -void HandleSerialRequests() { - if (Serial.available() >= 6) { // 2 headers + 1 command + 2 length + 1 checksum minimum - if (Serial.read() == HEADER1 && Serial.read() == HEADER2) { - uint8_t command = Serial.read(); - uint16_t length = (Serial.read() << 8) | Serial.read(); - - - if (length > MAX_PAYLOAD_SIZE) { - Serial.println("Payload too large"); - while (Serial.available()) Serial.read(); // flush junk - return; - } - - unsigned long start = millis(); - while (Serial.available() < length + 1) { - if (millis() - start > 1000) { // 1 second timeout - Serial.println("Serial timeout"); - return; - } - } - - - uint8_t checksum = command ^ (length >> 8) ^ (length & 0xFF); - - for (uint16_t i = 0; i < length; i++) { - payload[i] = Serial.read(); - checksum ^= payload[i]; - } - - uint8_t receivedChecksum = Serial.read(); - - if (checksum == receivedChecksum) { - handleCommand(command, payload, length); - } else { - Serial.println("Checksum mismatch"); - } - } +void handleProtocol() { + if (receivePacket()) { + dispatchCommand(); } } -void sendMessage(const String& payload, uint8_t command = CMD_MESSAGE) { - uint16_t length = payload.length(); - - uint8_t checksum = CMD_MESSAGE ^ (length >> 8) ^ (length & 0xFF); - for (int i = 0; i < length; i++) { - checksum ^= payload[i]; - } - - Serial.write(HEADER1); - Serial.write(HEADER2); - Serial.write(command); - Serial.write((length >> 8) & 0xFF); - Serial.write(length & 0xFF); - Serial.write((const uint8_t*)payload.c_str(), length); - Serial.write(checksum); -} - -void sendMessage(const uint8_t* payload, uint16_t length, uint8_t command = CMD_MESSAGE) { - uint8_t checksum = command ^ (length >> 8) ^ (length & 0xFF); - for (uint16_t i = 0; i < length; i++) { - checksum ^= payload[i]; - } - - Serial.write(HEADER1); - Serial.write(HEADER2); - Serial.write(command); - Serial.write((length >> 8) & 0xFF); - Serial.write(length & 0xFF); - Serial.write(payload, length); - Serial.write(checksum); -} - -void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) { - switch (command) { - case CMD_ID_REQUEST: // CMD_ID_REQUEST - handleIdRequest(); - break; - - case CMD_FILE_LIST: // - handleFileList(); - break; - - case CMD_LOAD_FILE: // - handleLoadFile(payload, length); - break; - - case CMD_DELETE_FILE: // - handleDeleteFile(payload, length); - break; - - case CMD_SAVE_FILE: - handleSaveFile(payload, length); - break; - - case CMD_SET_POSITION: - handleSetPosition(payload, length); - break; - - case CMD_PLAY_FILE: - handlePlayAnimation(payload, length); - break; - - case CMD_SCAN_CHANNEL: - handleScanChannel(payload, length); - break; - - case CMD_WRITE_DATA: - handleDataWrite(payload, length); - break; - - case CMD_START_POSITION_STREAM: - startPositionStream(payload, length); - break; - - case CMD_WRITE_CONFIG_UPDATE: - handleConfigUpdate(payload, length); - break; - - default: - Serial.print("Unknown command: "); - Serial.println(command, HEX); - break; - } -} - - -void startPositionStream(const uint8_t* payload, uint16_t length) { - bool start = payload[0] != 0; - - for (const Motor& motor : config.motors) { - servos[payload[0]]->setFeetechMode(motor.servoModel.major); // put feetech interface in correct mode - servos[0]->disableTorque(motor.motorID); - servos[0]->waitOnData1Byte(10); - } - - streamPositions = start; -} - -void handleConfigUpdate(const uint8_t* payload, uint16_t length) { - RobotConfig newConfig; - uint16_t offset = 0; - - // 🔹 Decode robot name - uint8_t nameLength = payload[offset++]; - newConfig.deviceName = ""; - - for (uint8_t i = 0; i < nameLength && offset < length; ++i) { - newConfig.deviceName += (char)payload[offset++]; - } - - // 🔹 Decode firmware version - if (offset + 2 > length) return; - newConfig.firmwareVersion.major = payload[offset++]; - newConfig.firmwareVersion.minor = payload[offset++]; - - // 🔹 Decode motor count - if (offset >= length) return; - uint8_t motorCount = payload[offset++]; - - // 🔹 Decode motors - for (uint8_t i = 0; i < motorCount && offset < length; ++i) { - if (offset + 5 > length) break; // MODEL(2) + ID(2) + nameLength(1) - - 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 < length; ++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); - } - - // 🔧 Now you can use `config` however you like - Serial.println("Robot name: " + newConfig.deviceName); - Serial.printf("Firmware: %d.%d\n", newConfig.firmwareVersion.major, config.firmwareVersion.minor); - Serial.printf("Motors: %d\n", newConfig.motors.size()); - - for (const Motor& m : newConfig.motors) { - Serial.printf("Motor %d (%s) - Model %d.%d\n", m.motorID, m.name.c_str(), m.servoModel.major, m.servoModel.minor); - } - - sendMessage(newConfig.deviceName); - config = newConfig; - config.saveToFFat(); -} - - - -void handleDataWrite(const uint8_t* payload, uint16_t length) { - uint8_t channel = payload[0]; - uint8_t id = payload[1]; - uint8_t writeByte = payload[2]; - - uint8_t model = servos[payload[0]]->getMajorModel(id); // config.getMotorModel(id); - sendMessage(String(model)); - servos[payload[0]]->setFeetechMode(model); // put feetech interface in correct mode - - - // Special case for ID changes, perform unlock, change, lock routine - if (writeByte == 5) { - sendMessage("CHANGING ID"); - servos[channel]->changeID(id, payload[4]); - return; - } - - // payload[4] indicates if its a single byte, or 2 bytes of information - if (payload[4] == 1) { - servos[channel]->write1Byte(id, writeByte, payload[4]); - uint8_t response = servos[channel]->waitOnData1Byte(10); - sendMessage(&response, 1, CMD_WRITE_DATA); - } else { - servos[channel]->write2Bytes(id, writeByte, payload[4] | (payload[5] << 8)); - uint16_t response = servos[channel]->waitOnData2Bytes(10); - uint8_t buffer[2]; - buffer[0] = (response >> 8) & 0xFF; // high byte - buffer[1] = response & 0xFF; // low byte - sendMessage(buffer, 2, CMD_WRITE_DATA); - } -} - -void handleIdRequest() { - std::vector payload = config.serializeToBytes(); - uint16_t length = payload.size(); - - // Compute checksum: XOR of CMD_ID_REQUEST, length bytes, and all payload bytes - uint8_t checksum = CMD_ID_REQUEST ^ (length >> 8) ^ (length & 0xFF); - for (uint8_t byte : payload) { - checksum ^= byte; - } - - // Send header - Serial.write(HEADER1); - Serial.write(HEADER2); - Serial.write(CMD_ID_REQUEST); - Serial.write((length >> 8) & 0xFF); - Serial.write(length & 0xFF); - - // Send payload - for (uint8_t byte : payload) { - Serial.write(byte); - } - - // Send checksum - Serial.write(checksum); -} - - -void handleFileList() { - File root = FFat.open("/"); - if (!root || !root.isDirectory()) { - sendFileListResponse(""); // empty payload - return; - } - - String payload = ""; - File file = root.openNextFile(); - - while (file) { - if (!file.isDirectory()) { - payload += String(file.name()) + "\n"; - } - file = root.openNextFile(); - } - - sendFileListResponse(payload); -} - -void sendFileListResponse(const String& payloadStr) { - uint16_t length = payloadStr.length(); - if (length > MAX_PAYLOAD_SIZE) { - Serial.println("File list too large"); - return; - } - - uint8_t checksum = CMD_FILE_LIST ^ (length >> 8) ^ (length & 0xFF); - for (uint16_t i = 0; i < length; i++) { - checksum ^= payloadStr[i]; - } - - Serial.write(HEADER1); - Serial.write(HEADER2); - Serial.write(CMD_FILE_LIST); - Serial.write((length >> 8) & 0xFF); - Serial.write(length & 0xFF); - Serial.write((const uint8_t*)payloadStr.c_str(), length); - Serial.write(checksum); -} - -void handleLoadFile(const uint8_t* payload, uint16_t length) { - if (length == 0 || length >= 128) { - Serial.println("Invalid filename"); - return; - } - - char filename[128]; - memcpy(filename, payload, length); - filename[length] = '\0'; - - File file = FFat.open(filename, "r"); - if (!file || !file.available()) { - Serial.println("File not found or empty"); - return; - } - - size_t fileSize = file.size(); - if (fileSize > 65535) { - Serial.println("File too large for single transfer"); - file.close(); - return; - } - - uint8_t* buffer = (uint8_t*)malloc(fileSize); - if (!buffer) { - Serial.println("Memory allocation failed"); - file.close(); - return; - } - - size_t bytesRead = file.read(buffer, fileSize); - file.close(); - - if (bytesRead != fileSize) { - Serial.println("File read error"); - free(buffer); - return; - } - - // 🔹 Compute checksum - uint8_t checksum = CMD_LOAD_FILE ^ (fileSize >> 8) ^ (fileSize & 0xFF); - for (size_t i = 0; i < fileSize; i++) { - checksum ^= buffer[i]; - } - - // 🔹 Send packet - Serial.write(HEADER1); - Serial.write(HEADER2); - Serial.write(CMD_LOAD_FILE); - Serial.write((fileSize >> 8) & 0xFF); - Serial.write(fileSize & 0xFF); - Serial.write(buffer, fileSize); - Serial.write(checksum); - - free(buffer); - Serial.println("File sent in one go"); -} - -void handleDeleteFile(const uint8_t* payload, uint16_t length) { - sendMessage("Deleting FILE"); - if (length < 1) { - Serial.println("Payload too short for filename length"); - sendMessage("Payload too short for filename length"); - return; - } - - // 🔹 Parse filename - uint16_t filenameLength = payload[0] | (payload[1] << 8); - if (length < 2 + filenameLength) { - Serial.println("Payload too short for filename"); - sendMessage("Payload too short for filename"); - return; - } - - - char filename[filenameLength + 1]; - - - memcpy(filename, payload + 2, filenameLength); - filename[filenameLength] = '\0'; - - deleteFile(FFat, ("/" + String(filename)).c_str()); - - - sendMessage(("File Deleted: " + String(filename)).c_str(), CMD_DELETE_FILE); -} - - -void handlePlayAnimation(const uint8_t* payload, uint16_t length) { - sendMessage("Playing FILE"); - if (length < 1) { - Serial.println("Payload too short for filename length"); - sendMessage("Payload too short for filename length"); - return; - } - - // 🔹 Parse filename - uint16_t filenameLength = payload[0] | (payload[1] << 8); - if (length < 2 + filenameLength) { - Serial.println("Payload too short for filename"); - sendMessage("Payload too short for filename"); - return; - } - - - char filename[filenameLength + 1]; - - memcpy(filename, payload + 2, filenameLength); - filename[filenameLength] = '\0'; - - // 🔹 Extract playback mode and repeat count - playMode = payload[2 + filenameLength]; - repeatsRemaining = payload[3 + filenameLength]; - - //deleteFile(FFat, ("/" + String(filename)).c_str()); - anim.clear(); - anim.loadFromFile(("/" + String(filename)).c_str()); - //playAnimation(anim); - - currentAnimation = &anim; - currentAnimation->setActive(true); // ✅ mark it as running - - sendMessage("File Played", CMD_PLAY_FILE); -} - -void handleSaveFile(const uint8_t* payload, uint16_t length) { - bool valid = parseAnimationPayload(payload, length, anim); - - if (valid) { - //Serial.println("Animation parsed successfully!"); - //anim.printKeyframes(); - } else { - //Serial.println("Failed to parse animation."); - length = 1; // Override length to 1 for fallback response - payload = nullptr; // We'll send a single 0x00 instead - } - - if (length > MAX_PAYLOAD_SIZE) { - Serial.println("File list too large"); - return; - } - - // Calculate checksum - uint8_t checksum = CMD_SAVE_FILE ^ (length >> 8) ^ (length & 0xFF); - if (valid) { - for (uint16_t i = 0; i < length; i++) { - checksum ^= payload[i]; - } - } else { - checksum ^= 0x00; - } - - // Send response - Serial.write(HEADER1); - Serial.write(HEADER2); - Serial.write(CMD_SAVE_FILE); - Serial.write((length >> 8) & 0xFF); - Serial.write(length & 0xFF); - - if (valid) { - for (uint16_t i = 0; i < length; i++) { - Serial.write(payload[i]); - } - } else { - Serial.write(0x00); - } - - Serial.write(checksum); -} - - - -bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& animation) { - sendMessage("SAVING FILE"); - if (length < 1) { - Serial.println("Payload too short for filename length"); - return false; - } - - // 🔹 Parse filename - uint16_t filenameLength = payload[0] | (payload[1] << 8); - if (length < 2 + filenameLength + 18) { - Serial.println("Payload too short for filename and header"); - return false; - } - - char filename[filenameLength + 1]; - - - memcpy(filename, payload + 2, filenameLength); - filename[filenameLength] = '\0'; - - - const uint8_t* ptr = payload + 2 + filenameLength; - - String msg = "SAVING FILE: "; - msg += filename; - sendMessage(msg); - // 🔹 Parse header - memcpy(animation.header.magic, ptr, 4); - if (strncmp(animation.header.magic, "ANIM", 4) != 0) { - Serial.println("Invalid magic header"); - - sendMessage("invalid magic header"); - return false; - } - - animation.header.frameCount = ptr[5] << 8 | ptr[4]; // little-endian ✅ - sendMessage(String(animation.header.frameCount)); - animation.header.version = ptr[6]; - animation.header.frameRate = ptr[7]; - memcpy(animation.header.reserved, ptr + 8, 8); - - uint16_t curveCount = ptr[17] << 8 | ptr[16]; // little-endian ✅ - Serial.print("curveCount: "); - Serial.println(curveCount); - - ptr += 18; - - if (length < (ptr - payload) + curveCount * sizeof(CurveSegment)) { - Serial.println("Payload too short for curve segments"); - sendMessage("Payload too short"); - return false; - } - - // 🔹 Parse curve segments - animation.clearAllCurves(); - for (uint16_t i = 0; i < curveCount; i++) { - CurveSegment seg; - memcpy(&seg, ptr, sizeof(CurveSegment)); - // Serial.print("Segment "); - // Serial.print(i); - // Serial.print(": motorID="); - // Serial.print(seg.motorID); - // Serial.print(", startTime="); - // Serial.print(seg.startTime); - // Serial.print(", endTime="); - // Serial.print(seg.endTime); - // Serial.print(", startPointY="); - // Serial.print(seg.startPointY); - // Serial.print(", startHandleX="); - // Serial.print(seg.startHandleX); - // Serial.print(", startHandleY="); - // Serial.print(seg.startHandleY); - // Serial.print(", endHandleX="); - // Serial.print(seg.endHandleX); - // Serial.print(", endHandleY="); - // Serial.print(seg.endHandleY); - // Serial.print(", endPointY="); - // Serial.println(seg.endPointY); - - - animation.addCurveSegment(seg); - ptr += sizeof(CurveSegment); - } - sendMessage(anim.printCurves()); - // for (int i = 0; i < 800; i++){ - // sendMessage(String(anim.getMotorPosition(11, i))); - // } - // ✅ Advance ptr to node graph payload - //ptr += curveCount * sizeof(CurveSegment); - - // 🔹 Parse node graph - uint8_t nodeCount = ptr[0]; - Serial.print("Node count: "); - Serial.println(nodeCount); - // sendMessage(String("Node count: ") + String(nodeCount)); - // sendMessage(String("ptr offset: ") + String(ptr - payload)); - - - uint16_t remainingLength = length - (ptr - payload); - if (remainingLength > 0) { - loadNodeGraph(ptr, remainingLength, animation.nodeGraph); - animation.nodeGraph.bindAnimationContext(&animation); - } else { - Serial.println("No node graph data found"); - } - sendMessage(printNodeGraph(animation.nodeGraph)); - - // currentAnimation = &animation; - // currentAnimation->setActive(true); // ✅ mark it as running - - // 🔹 Save using received filename - animation.saveToFile(("/" + String(filename)).c_str()); - - sendMessage("SAVED"); - return true; -} - - -void handleSetPosition(const uint8_t* payload, uint16_t length) { - if (length % 3 != 0) { - Serial.println("Invalid sync packet length"); - return; - } - - uint8_t motorCount = length / 3; - - // Dynamically allocate arrays - uint8_t ids[motorCount]; - uint16_t positions[motorCount]; - uint16_t speeds[motorCount]; - - for (uint8_t i = 0; i < motorCount; ++i) { - uint8_t motorId = payload[i * 3]; - uint16_t position = (payload[i * 3 + 2] << 8) | payload[i * 3 + 1]; - - ids[i] = motorId; - positions[i] = position; - speeds[i] = 0; - // Optional: update internal state or debug - // Serial.printf("Motor %d → %d\n", motorId, position); - } - - // FLIP BYTES FOR STS SERVOS - for (int i = 0; i < motorCount; i++) { - if (config.getMotorModel(ids[i]) == 9) { - positions[i] = flipBytes(positions[i]); - speeds[i] = map(speeds[i], 0, 4095, 0, 254); - speeds[i] = flipBytes(speeds[i]); - } else { - positions[i] = map(positions[i], 0, 4095, 0, 1023); - speeds[i] = map(speeds[i], 0, 4095, 0, 1000); - } - } - - // Send sync write to all motors - servos[0]->syncWritePos(ids, positions, speeds, motorCount); - - String readablePayload = encodeMotorPositionsReadable(ids, positions, motorCount); - sendMessage(readablePayload, CMD_MESSAGE); -} - - -String encodeMotorPositionsReadable(const uint8_t* ids, const uint16_t* positions, uint8_t motorCount) { - String payload = ""; - - for (uint8_t i = 0; i < motorCount; ++i) { - payload += "ID:"; - payload += String(ids[i]); - payload += ",POS:"; - payload += String(positions[i]); - - if (i < motorCount - 1) { - payload += ";"; // separate entries - } - } - - return payload; -} - - - - -// Scans 0-254 and responds with the channel and ID as each successful ping is received -// Signals end by responding with channel and 255 -void handleScanChannel(const uint8_t* payload, uint16_t length) { - if (length != 1) { - sendMessage("Length of scanChannel Request Wrong"); - return; - } - - - for (int i = 0; i < 254; i++) { - servos[payload[0]]->sendPing(i); - uint8_t val = servos[payload[0]]->waitOnData1Byte(10); - - if (val != 0) { - uint8_t response[44]; // Adjusted size to fit all values - response[0] = payload[0]; // channel - response[1] = i; // responding address - - uint16_t model = servos[payload[0]]->getModel(i); - servos[payload[0]]->setFeetechMode(model); // put feetech interface in correct mode - uint16_t minAngleLimit = servos[payload[0]]->getMinAngleLimit(i); - uint16_t maxAngleLimit = servos[payload[0]]->getMaxAngleLimit(i); - uint16_t position = servos[payload[0]]->getPosition(i); - uint8_t CWDeadZone = servos[payload[0]]->getCWDeadZone(i); - uint8_t CCWDeadZone = servos[payload[0]]->getCCWDeadZone(i); - uint16_t offset = servos[payload[0]]->getOffset(i); - uint8_t mode = servos[payload[0]]->getMode(i); - uint8_t torqueEnable = servos[payload[0]]->getTorqueEnable(i); - uint8_t acceleration = servos[payload[0]]->getAcceleration(i); - uint16_t goalPosition = servos[payload[0]]->getGoalPosition(i); - uint16_t goalTime = servos[payload[0]]->getGoalTime(i); - uint16_t goalSpeed = servos[payload[0]]->getGoalSpeed(i); - uint8_t lock = servos[payload[0]]->getLock(i); - int16_t speed = servos[payload[0]]->getSpeed(i); - uint16_t load = servos[payload[0]]->getLoad(i); - uint8_t temperature = servos[payload[0]]->getTemperature(i); - uint8_t moving = servos[payload[0]]->getMoving(i); - uint8_t current = servos[payload[0]]->getCurrent(i); - uint8_t voltage = servos[payload[0]]->getVoltage(i); - - // Pack values into response - response[2] = (model >> 8) & 0xFF; - response[3] = model & 0xFF; - - response[4] = (minAngleLimit >> 8) & 0xFF; - response[5] = minAngleLimit & 0xFF; - - response[6] = (maxAngleLimit >> 8) & 0xFF; - response[7] = maxAngleLimit & 0xFF; - - response[8] = (position >> 8) & 0xFF; - response[9] = position & 0xFF; - - response[10] = CWDeadZone; - response[11] = CCWDeadZone; - - response[12] = (offset >> 8) & 0xFF; - response[13] = offset & 0xFF; - - response[14] = mode; - response[15] = torqueEnable; - response[16] = acceleration; - - response[17] = (goalPosition >> 8) & 0xFF; - response[18] = goalPosition & 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] = temperature; - response[29] = moving; - response[30] = (current >> 8) & 0xFF; - response[31] = current & 0xFF; - - response[32] = voltage; - - - // You can continue adding more fields here if needed... - - sendMessage(response, 33, CMD_SCAN_CHANNEL); // updated length - } - } - uint8_t r[2]; - r[0] = payload[0]; // channel - r[1] = 255; // responding address - - sendMessage(r, 2, CMD_SCAN_CHANNEL); - - - // std::vector successfulAddresses; - // servos[payload[0]]->pingAll(successfulAddresses); - // std::vector response; - // response.push_back(payload[0]); // channel - // response.push_back(successfulAddresses.size()); // count - - // for (uint8_t address : successfulAddresses) { - // response.push_back(address); - // } - - // sendMessage(response.data(), response.size(), CMD_SCAN_CHANNEL); -} - - -// Target packet: 0xAA, 0x55, 0x01, 0x00, 0x00, 0x01 -const uint8_t targetPacket[6] = {0xAA, 0x55, 0x01, 0x00, 0x00, 0x01}; -uint8_t sniffBuffer[6]; -uint8_t sniffIndex = 0; - -void sniffByte(uint8_t b) { - // Push into buffer - sniffBuffer[sniffIndex++] = b; - if (sniffIndex == 6) { - // Compare with target - bool match = true; - for (int i = 0; i < 6; i++) { - if (sniffBuffer[i] != targetPacket[i]) { - match = false; - break; - } - } - if (match) { - Serial.println("Detected FEETECH packet 0xAA 0x55 0x01 0x00 0x00 0x01!"); - mode = MODE_NORMAL; - handleIdRequest(); - return; - } - // Slide window: keep last 5 bytes - for (int i = 1; i < 6; i++) sniffBuffer[i-1] = sniffBuffer[i]; - sniffIndex = 5; - } -} - -bool flip = false; -unsigned long lastSend = 0; -void loop() { - // if (mode == MODE_FEETECH) { - // if (Serial.available()) { - // char c = Serial.read(); - // Serial1.write(c); - // sniffByte((uint8_t)c); // sniff traffic PC → UART - // } - - // // Forward data from Serial1 (UART device) to Serial (PC) - // if (Serial1.available()) { - // char c = Serial1.read(); - // Serial.write(c); - // } - // return; - // } - - - runNodeAnimation(); - - - HandleSerialRequests(); - - - - - if (millis() - lastSend > 50) { - // Update config motor positions - for (const Motor& motor : config.motors) { - servos[0]->setFeetechMode(motor.servoModel.major); // put feetech interface in correct mode - uint16_t position = servos[0]->getPosition(motor.motorID); - config.setMotorPosition(motor.motorID, position); - //Serial.print(position); - //Serial.print("\t"); - } - //Serial.println(); - lastSend = millis(); - - - - // for (const Motor& motor : config.motors) { - // uint16_t position = motor.position; - // uint8_t v1 = position >> 8; - // uint8_t v2 = position & 0xFF; - // Serial.print(v1); - // Serial.print("\t"); - // Serial.print(v2); - // Serial.print(",\t"); - // //Serial.print(position); - // //Serial.print(" "); - // //Serial.print((v1 << 8) | v2); - // } - // Serial.println(); - } - - if (streamPositions) { - if (millis() - lastStreamPositions > 50) { - lastStreamPositions = millis(); - SendMotorPositions(); - } - } -} - - +// ============================================================================ +// Animation Playback +// ============================================================================ void runNodeAnimation() { static uint32_t lastTickTime = 0; static uint32_t currentTick = 0; - const uint16_t FRAME_INTERVAL_MS = 1000 / 48; + static bool wasActive = false; - if (!currentAnimation) return; // ✅ Prevent crash - if (!currentAnimation->isActive()) return; + if (!animState.current || !animState.current->isActive()) { + wasActive = false; + return; + } config.enableAllMotors(); uint32_t now = millis(); - if (now - lastTickTime >= FRAME_INTERVAL_MS) { + if (now - lastTickTime < FRAME_INTERVAL_MS) return; lastTickTime = now; - currentAnimation->nodeGraph.tick(currentTick, *currentAnimation); - - // if (currentTick == 100){ - // message += String(anim.getMotorPosition(10, currentTick)); - // message += "\n"; - // } - - auto outputs = currentAnimation->nodeGraph.getServoOutputs(); + // Tick the node graph + animState.current->nodeGraph.tick(currentTick, *animState.current); + auto outputs = animState.current->nodeGraph.getServoOutputs(); + // Collect motor commands std::vector motorIDs; std::vector positions; std::vector speeds; - String message = String(currentTick) + String("\n"); for (const auto& [motorID, value] : outputs) { if (value != 65535) { @@ -1061,203 +91,264 @@ void runNodeAnimation() { positions.push_back(value); speeds.push_back(0); config.setMotorPosition(motorID, value); - if (config.setMotorEnabled(motorID, true)) { - servos[0]->enableTorque(motorID); - } + config.setMotorEnabled(motorID, true); } else { + // Only disable torque for motors that should be limp if (config.setMotorEnabled(motorID, false)) { - servos[0]->disableTorque(motorID); - } - } - message += "Motor "; - message += String(motorID); - message += " → "; - message += String(value); - message += "\n"; - } - - uint8_t motorCount = motorIDs.size(); // ✅ number of motors - - if (currentTick % 20 == 0) { - sendMessage(message); - } - - // FLIP BYTES FOR STS SERVOS - for (int i = 0; i < motorCount; i++) { - if (config.getMotorModel(motorIDs[i]) == 9) { - positions[i] = flipBytes(positions[i]); - speeds[i] = map(speeds[i], 0, 4095, 0, 254); - speeds[i] = flipBytes(speeds[i]); - } else { - positions[i] = map(positions[i], 0, 4095, 0, 1023); - speeds[i] = map(speeds[i], 0, 4095, 0, 1000); + servoManager[0]->disableTorque(motorID); } } + } - - // ✅ Send to servo controller - servos[0]->syncWritePos(motorIDs.data(), positions.data(), speeds.data(), motorCount); - + // Send all positions in one sync write - motors move together! + if (!motorIDs.empty()) { + servoManager.syncWritePositions( + motorIDs.data(), + positions.data(), + speeds.data(), + motorIDs.size(), + config, + 0 + ); + } currentTick++; - if (currentTick > currentAnimation->getFrameCount()) { - switch (playMode) { + // Handle animation end + if (currentTick > animState.current->getFrameCount()) { + switch (animState.playMode) { case PLAY_ONCE: - currentAnimation->setActive(false); + animState.stop(); break; case PLAY_LOOP: - + // Continue looping break; case PLAY_REPEAT: - repeatsRemaining--; - if (repeatsRemaining == 0) { - currentAnimation->setActive(false); + if (--animState.repeatsRemaining == 0) { + animState.stop(); } break; + default: + break; } currentTick = 0; - } } } +// ============================================================================ +// Motor Position Updates +// ============================================================================ +void updateMotorPositions() { + static unsigned long lastUpdate = 0; -void SendMotorPositions() { - std::vector payload; + if (millis() - lastUpdate < MOTOR_UPDATE_INTERVAL_MS) return; + lastUpdate = millis(); for (const Motor& motor : config.motors) { - payload.push_back(motor.motorID); - uint16_t position = motor.position; - payload.push_back(position & 0xFF); // Low byte - payload.push_back(position >> 8); // High byte - //Serial.print(position); - - //Serial.print("\t"); - // Serial.print(position & 0xFF); - // Serial.print(",\t"); - } - //Serial.println(); - - sendMessage(payload.data(), payload.size(), POSITION_STREAM); -} - - -void PrintFileList() { - File root = FFat.open("/"); - if (!root || !root.isDirectory()) { - Serial.println("Failed to open FFat root directory"); - return; - } - - Serial.println("Files in FFat:"); - - File file = root.openNextFile(); - while (file) { - Serial.print(" "); - Serial.print(file.name()); - Serial.print(" | Size: "); - Serial.println(file.size()); - file = root.openNextFile(); - } - - Serial.println("End of file list."); -} - -void ClearFiles() { - File root = FFat.open("/"); - if (!root || !root.isDirectory()) { - Serial.println("Failed to open FFat root directory"); - return; - } - - Serial.println("Files in FFat:"); - - File file = root.openNextFile(); - while (file) { - String filename = "/" + String(file.name()); // Add leading slash - Serial.print(" Deleting: "); - Serial.println(filename); - file.close(); // Close before deleting - deleteFile(FFat, filename.c_str()); // Use corrected path - file = root.openNextFile(); - } - - Serial.println("FFat cleanup complete."); -} - - - -void deleteFile(fs::FS& fs, const char* path) { - Serial.printf("Deleting file: %s\r\n", path); - if (fs.remove(path)) { - Serial.println("- file deleted"); - } else { - Serial.println("- delete failed"); + servoManager[0]->setFeetechMode(motor.servoModel.major); + uint16_t position = servoManager[0]->getPosition(motor.motorID); + config.setMotorPosition(motor.motorID, position); } } +void handleMotorStreaming() { + if (motorStream.shouldStream()) { + sendMotorPositions(); + } +} -void playAnimation(Animation& animation) { - uint16_t durationCS = animation.getFrameCount(); - const uint8_t fps = 48; - const uint32_t frameIntervalMS = 1000 / fps; // ~20.83 ms - const uint32_t totalDurationMS = durationCS * 10; +// ============================================================================ +// Heartbeat +// ============================================================================ - uint32_t startTime = millis(); - uint32_t nextFrameTime = startTime; +void sendHeartbeat() { + static unsigned long lastHeartbeat = 0; + + if (millis() - lastHeartbeat < HEARTBEAT_INTERVAL_MS) return; + lastHeartbeat = millis(); - while (millis() - startTime < totalDurationMS) { - uint32_t currentTime = millis(); + // Pack state: uptime(4) + flags(2) + uint8_t payload[6]; + uint32_t uptime = millis() / 1000; + uint16_t flags = 0; + + // Build flags + if (imu.isReady()) flags |= 0x01; + if (animState.current && animState.current->isActive()) flags |= 0x02; + if (motorStream.active) flags |= 0x04; + if (sensors.isIMUStreamEnabled()) flags |= 0x08; + if (sensors.isRadarStreamEnabled()) flags |= 0x10; - if (currentTime >= nextFrameTime) { - uint16_t timeCS = (currentTime - startTime) / 10; + 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; - //for (uint8_t motorID = 0; motorID < NUM_CHANNELS; motorID++) { - uint16_t pos = animation.getMotorPosition(0, timeCS); - pos = pos / 4; - servos[0]->sendWritePos(10, pos); + sendPacket(Tag::STATE, payload, 6); +} - Serial.println(pos); - //} +// ============================================================================ +// Debug Test Mode +// ============================================================================ - nextFrameTime += frameIntervalMS; +// Set to true to run IMU test on startup +#define DEBUG_IMU_TEST false + +void runIMUTest() { + static unsigned long lastPrint = 0; + + if (millis() - lastPrint < 200) return; // Print every 200ms + lastPrint = millis(); + + Serial.println("--- IMU TEST ---"); + + // Check IMU status + Serial.print("IMU Ready: "); + Serial.println(imu.isReady() ? "YES" : "NO"); + + if (imu.isReady()) { + Serial.print(" Heading: "); + Serial.print(imu.getHeading(), 1); + Serial.print("° Pitch: "); + Serial.print(imu.getPitch(), 1); + Serial.print("° Roll: "); + Serial.print(imu.getRoll(), 1); + Serial.println("°"); + + // Calculate what the node would output + float pitch = constrain(imu.getPitch(), -90.0f, 90.0f); + uint16_t pitchValue = (uint16_t)(((pitch + 90.0f) / 180.0f) * 4095.0f); + Serial.print(" Pitch as node value: "); + Serial.println(pitchValue); + } + + // Check if animation is loaded + Serial.print("Animation Active: "); + Serial.println((animState.current && animState.current->isActive()) ? "YES" : "NO"); + + if (animState.current) { + Serial.print(" Nodes: "); + Serial.print(animState.current->nodeGraph.nodes.size()); + Serial.print(" Connections: "); + Serial.println(animState.current->nodeGraph.connections.size()); + + // Print each node's output + for (Node* node : animState.current->nodeGraph.nodes) { + Serial.print(" Node "); + Serial.print(node->id); + Serial.print(" (type "); + Serial.print(node->type); + Serial.print("): in="); + Serial.print(node->inputValue); + Serial.print(" out="); + Serial.println(node->outputValue); } - - // Optional: yield or small delay to avoid busy loop - delay(1); - } - - // Optional: reset motors to center - // for (uint8_t motorID = 0; motorID < NUM_CHANNELS; motorID++) { - // servos[0]->sendWritePos(motorID, 2048); - // } -} - - - -void SCSPingAll() { - std::vector successfulAddresses; - servos[0]->pingAll(successfulAddresses); - - // Now successfulAddresses contains all successful pings - Serial.println("Successful Addresses:"); - for (uint8_t address : successfulAddresses) { - Serial.print(address); + + // Print servo outputs + auto outputs = animState.current->nodeGraph.getServoOutputs(); + Serial.print(" Servo outputs: "); + for (const auto& [motorID, value] : outputs) { + Serial.print("M"); + Serial.print(motorID); + Serial.print("="); + Serial.print(value); Serial.print(" "); } Serial.println(); } -void STSPingAll() { - std::vector successfulAddresses; - servos[1]->pingAll(successfulAddresses); - - // Now successfulAddresses contains all successful pings - Serial.println("Successful Addresses:"); - for (uint8_t address : successfulAddresses) { - Serial.print(address); - Serial.print(" "); - } Serial.println(); } + +// ============================================================================ +// Setup +// ============================================================================ + +void setup() { + // Serial setup (buffer size must be set before begin) + Serial.setRxBufferSize(8192); + Serial.begin(1000000); + + // 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"); + + // Load or create robot config + if (config.loadOrCreateDefault()) { + Serial.println("[HansonServo] Config loaded: " + config.deviceName); + } else { + Serial.println("[HansonServo] Config init failed"); + } + + // DEBUG: Load and play IMU test animation + #if DEBUG_IMU_TEST + Serial.println("[DEBUG] Loading /imutest.anim..."); + if (animState.animation.loadFromFile("/imutest.anim")) { + Serial.println("[DEBUG] Animation loaded successfully!"); + Serial.print("[DEBUG] Nodes: "); + Serial.println(animState.animation.nodeGraph.nodes.size()); + Serial.print("[DEBUG] Connections: "); + Serial.println(animState.animation.nodeGraph.connections.size()); + + // Print node details + for (Node* node : animState.animation.nodeGraph.nodes) { + Serial.print("[DEBUG] Node "); + Serial.print(node->id); + Serial.print(": type="); + Serial.println(node->type); + } + + animState.play(PLAY_LOOP); + Serial.println("[DEBUG] Animation started in LOOP mode"); + } else { + Serial.println("[DEBUG] Failed to load animation!"); + } + #endif + + Serial.println("[HansonServo] Ready"); + Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16"); +} + +// ============================================================================ +// Main Loop +// ============================================================================ + +void loop() { + // Protocol handling + handleProtocol(); + + // Animation playback + runNodeAnimation(); + + // Motor position updates + updateMotorPositions(); + handleMotorStreaming(); + + // Sensor updates and streaming + sensors.update(); + + // Heartbeat + sendHeartbeat(); + + // DEBUG: Print IMU test info + #if DEBUG_IMU_TEST + runIMUTest(); + #endif +} diff --git a/PROTOCOL_MIGRATION.md b/PROTOCOL_MIGRATION.md new file mode 100644 index 0000000..da4209c --- /dev/null +++ b/PROTOCOL_MIGRATION.md @@ -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('> 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(' + +// 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 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(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 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); + } +} diff --git a/commands.h b/commands.h new file mode 100644 index 0000000..405517a --- /dev/null +++ b/commands.h @@ -0,0 +1,83 @@ +#pragma once +#include +#include +#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); diff --git a/motorcontrol.cpp b/motorcontrol.cpp new file mode 100644 index 0000000..9f9cb47 --- /dev/null +++ b/motorcontrol.cpp @@ -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(ids), + positions, + speeds, + count + ); +} + diff --git a/motorcontrol.h b/motorcontrol.h new file mode 100644 index 0000000..59d12e7 --- /dev/null +++ b/motorcontrol.h @@ -0,0 +1,60 @@ +#pragma once +#include +#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; + diff --git a/nodegraph.cpp b/nodegraph.cpp index 06c66e4..5ffe763 100644 --- a/nodegraph.cpp +++ b/nodegraph.cpp @@ -2,9 +2,12 @@ #include "animation.h" #include "noise.h" #include "RobotConfig.h" +#include "sensors.h" #include 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; } } diff --git a/nodegraph.h b/nodegraph.h index 8bac86c..4870bae 100644 --- a/nodegraph.h +++ b/nodegraph.h @@ -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) }; diff --git a/protocol.cpp b/protocol.cpp new file mode 100644 index 0000000..b68a7b3 --- /dev/null +++ b/protocol.cpp @@ -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; +} diff --git a/protocol.h b/protocol.h new file mode 100644 index 0000000..2cc9ca4 --- /dev/null +++ b/protocol.h @@ -0,0 +1,118 @@ +#pragma once +#include + +// ============================================================================ +// 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; +} diff --git a/sensors.cpp b/sensors.cpp new file mode 100644 index 0000000..3447310 --- /dev/null +++ b/sensors.cpp @@ -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); +} + diff --git a/sensors.h b/sensors.h new file mode 100644 index 0000000..275eca0 --- /dev/null +++ b/sensors.h @@ -0,0 +1,121 @@ +#pragma once +#include +#include + +// ============================================================================ +// 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; +