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; +