From 3b810e79c5d0015b293cd5016551d2b5c66c1964 Mon Sep 17 00:00:00 2001 From: Jake Date: Sun, 28 Sep 2025 14:59:29 +0800 Subject: [PATCH] batch sending files, broken keyframe data --- HansonServo.ino | 341 +++++++++++++++++++++++++++++++++++++++++++++++- animation.cpp | 201 ++++++++++++++++++++++++++++ animation.h | 56 ++++++++ feetech.cpp | 2 +- webpage.h | 34 +++++ 5 files changed, 627 insertions(+), 7 deletions(-) create mode 100644 animation.cpp create mode 100644 animation.h create mode 100644 webpage.h diff --git a/HansonServo.ino b/HansonServo.ino index ee0a9f8..c547de6 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -1,10 +1,28 @@ +#include #include "feetech.h" +#include "animation.h" + +#define DEVICE_NAME "Little Sophia" +#define FIRMWARE_VERSION "0.0.1" + +#define HEADER1 0xAA +#define HEADER2 0x55 + +#define CMD_ID_REQUEST 0x01 +#define CMD_FILE_LIST 0x02 +#define CMD_LOAD_FILE 0x03 +#define CMD_DELETE_FILE 0x04 +#define CMD_LOAD_FILE_CHUNK 0x05 + + // ESP32 S2 PINOUT #define RX_PIN 17 // DI #define TX_PIN 18 // RO #define DE_PIN 33 // Driver Enable #define RE_PIN 3 // Receiver Enable +Animation sweep; +Animation stagger; Feetech servos = Feetech(Serial1, DE_PIN, RE_PIN, TX_PIN, RX_PIN); @@ -12,22 +30,55 @@ uint16_t flipBytes(uint16_t value) { return (value >> 8) | (value << 8); } -uint8_t ids[] = { 1, 10, 11, 15 }; -uint16_t pos1[] = { 0, 0, 0, 0 }; -uint16_t pos2[] = { 1023, 1023, 1023, 4095 }; +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 }; void setup() { Serial.begin(115200); - for (int i = 0; i < 10; i++) { + for (int i = 0; i < 5; i++) { Serial.println(i); delay(500); } + pos2[3] = flipBytes(pos2[3]); servos.begin(); + if (!FFat.begin()) { + Serial.println("FFat mount failed"); + return; + } + + // sweep.clear(); + // sweep.createSampleSweep(4); + // sweep.saveToFile("/sweep.anim"); + // sweep.clear(); + // sweep.createStaggeredSweep(4); + // sweep.saveToFile("/stagger.anim"); + // delay(9999); + + // anim.clear(); + // if (!sweep.loadFromFile("/sweep.anim")) { + // Serial.println("Failed to load animation"); + // return; + // } else { + // Serial.println("Loaded sweep anim"); + // } + + // if (!stagger.loadFromFile("/stagger.anim")) { + // Serial.println("Failed to load animation"); + // return; + // } else { + // Serial.println("Loaded stagger anim"); + // } + + + // Serial.println(anim.getFrame(0, 0)); // Should show saved value + + //SetID(11, 14); } void SetID(uint8_t oldID, uint8_t newID) { @@ -45,12 +96,290 @@ void SetID(uint8_t oldID, uint8_t newID) { delay(1000); } +unsigned long lastSend = 0; void loop() { - // put your main code here, to run repeatedly: - PingAll(); + HandleSerialRequests(); + // put your main code here, to run repeatedly: + //PingAll(); + + + // playAnimation(sweep); + // playAnimation(stagger); + // playLayeredAnimation(sweep, stagger); + + + // servos.syncWritePos(ids, pos1, 5); + // delay(1000); + + // servos.syncWritePos(ids, pos2, 5); + // delay(1000); + + if (millis() - lastSend > 1000) { + //sendMessageFromESP32(String(millis())); + //PrintFileList(); + lastSend = millis(); + } } +void HandleSerialRequests() { + if (Serial.available() >= 4) { + if (Serial.read() == HEADER1 && Serial.read() == HEADER2) { + uint8_t command = Serial.read(); + uint8_t length = Serial.read(); + + String payload = ""; + for (int i = 0; i < length; i++) { + while (!Serial.available()) + ; + payload += (char)Serial.read(); + } + + handleCommand(command, payload); + } + } +} + +void handleCommand(uint8_t command, const String& payload) { + switch (command) { + case CMD_ID_REQUEST: + sendIdPacket(); + break; + case CMD_FILE_LIST: + sendFileList(); + break; + case CMD_LOAD_FILE: + sendFileContent(payload); + break; + case CMD_DELETE_FILE: + deleteFile(payload); + break; + //default: + //Serial.println("{\"error\":\"Unknown command\"}"); + } +} + +void sendReply(uint8_t command, const String& payload) { + uint16_t length = payload.length(); // Supports up to 65,535 bytes + + // Calculate checksum using both length bytes + uint8_t checksum = command ^ (length >> 8) ^ (length & 0xFF); + for (int i = 0; i < length; i++) { + checksum ^= payload[i]; + } + + // Send packet with 2-byte length + Serial.write(HEADER1); + Serial.write(HEADER2); + Serial.write(command); + Serial.write((length >> 8) & 0xFF); // High byte + Serial.write(length & 0xFF); // Low byte + Serial.write((const uint8_t*)payload.c_str(), length); + Serial.write(checksum); +} + + +void sendChunkReply(uint8_t command, const String& filename, const uint8_t* chunkData, size_t chunkSize, size_t offset, size_t totalSize) { + // Build JSON metadata + String payload = "{\"file\":\"" + filename + "\",\"offset\":" + offset + ",\"totalSize\":" + totalSize + ",\"chunk\":["; + + for (size_t i = 0; i < chunkSize; i++) { + payload += String(chunkData[i]); + if (i < chunkSize - 1) payload += ","; + } + + payload += "]}"; + + // Calculate payload length + uint16_t length = payload.length(); + + // Calculate checksum + uint8_t checksum = command ^ (length >> 8) ^ (length & 0xFF); + for (int i = 0; i < length; i++) { + checksum ^= payload[i]; + } + + // Send packet + Serial.write(HEADER1); + Serial.write(HEADER2); + Serial.write(command); + Serial.write((length >> 8) & 0xFF); // High byte + Serial.write(length & 0xFF); // Low byte + Serial.write((const uint8_t*)payload.c_str(), length); + Serial.write(checksum); +} + + + +void sendIdPacket() { + String payload = "{\"name\":\"" + String(DEVICE_NAME) + "\",\"version\":\"" + String(FIRMWARE_VERSION) + "\"}"; + sendReply(CMD_ID_REQUEST, payload); +} + + +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 sendFileList() { + File root = FFat.open("/"); + if (!root || !root.isDirectory()) { + sendReply(CMD_FILE_LIST, "[]"); + return; + } + + File file = root.openNextFile(); + String payload = "["; + + bool first = true; + while (file) { + if (!file.isDirectory()) { + if (!first) payload += ","; + payload += "\"" + String(file.name()) + "\""; + first = false; + } + file = root.openNextFile(); + } + + payload += "]"; + sendReply(CMD_FILE_LIST, payload); +} + +void sendFileContent(const String& filename) { + sendFileInChunks(filename); + return; + File file = FFat.open(filename, FILE_READ); + if (!file) { + sendReply(CMD_LOAD_FILE, "{\"error\":\"File not found\"}"); + return; + } + + String raw; + while (file.available()) { + raw += (char)file.read(); + } + file.close(); + + String encoded = base64::encode(raw); // This must be base64 + + String payload = "{\"file\":\"" + filename + "\",\"content\":\"" + encoded + "\"}"; + sendReply(CMD_LOAD_FILE, payload); +} + +void sendFileInChunks(const String& filename) { + File file = FFat.open(filename, FILE_READ); + if (!file) { + sendReply(CMD_LOAD_FILE, "{\"error\":\"File not found\"}"); + return; + } + + const size_t chunkSize = 512; + size_t totalSize = file.size(); + size_t offset = 0; + size_t chunksSent = 0; + + unsigned long startTime = millis(); + + while (offset < totalSize) { + size_t remaining = totalSize - offset; + size_t thisChunk = remaining < chunkSize ? remaining : chunkSize; + + uint8_t buffer[thisChunk]; + file.read(buffer, thisChunk); + + sendChunkReply(CMD_LOAD_FILE_CHUNK, filename, buffer, thisChunk, offset, totalSize); + + offset += thisChunk; + chunksSent++; + delay(10); // Optional pacing + } + + file.close(); + + unsigned long duration = millis() - startTime; + + String finalReply = "{\"status\":\"complete\",\"file\":\"" + filename + + "\",\"chunks\":" + chunksSent + + ",\"bytesSent\":" + totalSize + + ",\"durationMs\":" + duration + "}"; + + sendReply(CMD_LOAD_FILE, finalReply); +} + + + +void deleteFile(const String& filename) { + String payload = "{\"deleted\":\"" + filename + "\"}"; + sendReply(CMD_DELETE_FILE, payload); +} + +void playAnimation(Animation& anim) { + uint16_t positions[NUM_CHANNELS]; + const uint16_t frameCount = anim.getFrameCount(); + const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND; // 20 ms + + uint32_t nextFrameTime = millis(); + + for (uint16_t frame = 0; frame < frameCount; frame++) { + // Wait until it's time for the next frame + while (millis() < nextFrameTime) { + // Optional: yield or do background tasks here + delay(1); + } + + // Send frame to servos + if (anim.getFramePositions(frame, positions)) { + servos.syncWritePos(ids, positions, NUM_CHANNELS); + } + + // Schedule next frame + nextFrameTime += frameDelay; + } +} + +void playLayeredAnimation(Animation& base, Animation& overlay) { + uint16_t basePositions[NUM_CHANNELS]; + uint16_t overlayPositions[NUM_CHANNELS]; + uint16_t finalPositions[NUM_CHANNELS]; + + const uint16_t frameCount = base.getFrameCount(); + const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND; + uint32_t nextFrameTime = millis(); + + for (uint16_t frame = 0; frame < frameCount; frame++) { + while (millis() < nextFrameTime) delay(1); + + base.getFramePositions(frame, basePositions); + overlay.getFramePositions(frame, overlayPositions); + + for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { + finalPositions[ch] = (basePositions[ch] + overlayPositions[ch]) / 2; + } + + servos.syncWritePos(ids, finalPositions, NUM_CHANNELS); + nextFrameTime += frameDelay; + } +} + + + void PingAll() { std::vector successfulAddresses; servos.pingAll(successfulAddresses); diff --git a/animation.cpp b/animation.cpp new file mode 100644 index 0000000..4ecf5dc --- /dev/null +++ b/animation.cpp @@ -0,0 +1,201 @@ +#include "animation.h" + +Animation::Animation() { + clear(); + memcpy(header.magic, "ANIM", 4); + header.version = 1; + header.frameRate = FRAMES_PER_SECOND; + header.frameCount = MAX_FRAMES; + memset(header.reserved, 0, sizeof(header.reserved)); +} + +void Animation::setFrame(uint16_t frameIndex, uint16_t channel, uint16_t value) { + if (frameIndex < MAX_FRAMES && channel < NUM_CHANNELS) { + data[frameIndex][channel] = value; + } +} + +uint16_t Animation::getFrame(uint16_t frameIndex, uint16_t channel) const { + if (frameIndex < MAX_FRAMES && channel < NUM_CHANNELS) { + return data[frameIndex][channel]; + } + return 0; +} + +bool Animation::getFramePositions(uint16_t frameIndex, uint16_t* outPositions) { + if (frameIndex >= header.frameCount) return false; + + for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { + outPositions[ch] = getFrame(frameIndex, ch); + } + return true; +} + +void Animation::addKeyframe(uint8_t motorId, uint16_t frame, uint16_t position) { + keyframes.push_back({ motorId, frame, position }); +} + +const std::vector& Animation::getKeyframes() const { + return keyframes; +} + + + +void Animation::clear() { + memset(data, 0, sizeof(data)); +} + +uint16_t* Animation::getRawData() { + return &data[0][0]; +} + +size_t Animation::getSize() const { + return sizeof(data); +} + +uint16_t Animation::getFrameCount() const { + return header.frameCount; +} + +bool Animation::saveToFile(const char* filename) { + // Auto-detect actual frame count + uint16_t lastFrame = 0; + for (uint16_t frame = 0; frame < MAX_FRAMES; frame++) { + for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { + if (data[frame][ch] != 0) { + lastFrame = frame; + break; + } + } + } + header.frameCount = lastFrame + 1; // +1 because frame index starts at 0 + + File file = FFat.open(filename, FILE_WRITE); + if (!file) return false; + + // Write header and motion data + file.write((uint8_t*)&header, sizeof(header)); + file.write((uint8_t*)data, sizeof(data)); + + // Write keyframe count + uint16_t keyframeCount = keyframes.size(); + file.write((uint8_t*)&keyframeCount, sizeof(keyframeCount)); + + // Write keyframes + for (const Keyframe& kf : keyframes) { + file.write(kf.motorId); + file.write((uint8_t*)&kf.frame, sizeof(kf.frame)); + file.write((uint8_t*)&kf.position, sizeof(kf.position)); + } + + file.close(); + return true; +} + + +bool Animation::loadFromFile(const char* filename) { + File file = FFat.open(filename, FILE_READ); + if (!file) return false; + + // Read and validate header + AnimationHeader tempHeader; + if (file.read((uint8_t*)&tempHeader, sizeof(tempHeader)) != sizeof(tempHeader)) { + file.close(); + return false; + } + + if (strncmp(tempHeader.magic, "ANIM", 4) != 0 || tempHeader.version != 1) { + file.close(); + return false; + } + + header = tempHeader; + + // Read motion data + size_t expectedSize = sizeof(data); + if (file.read((uint8_t*)data, expectedSize) != expectedSize) { + file.close(); + return false; + } + + // Read keyframe count + uint16_t keyframeCount; + if (file.read((uint8_t*)&keyframeCount, sizeof(keyframeCount)) != sizeof(keyframeCount)) { + file.close(); + return false; + } + + // Read keyframes + keyframes.clear(); + for (uint16_t i = 0; i < keyframeCount; i++) { + Keyframe kf; + if (file.read(&kf.motorId, 1) != 1 || + file.read((uint8_t*)&kf.frame, sizeof(kf.frame)) != sizeof(kf.frame) || + file.read((uint8_t*)&kf.position, sizeof(kf.position)) != sizeof(kf.position)) { + file.close(); + return false; + } + keyframes.push_back(kf); + } + + file.close(); + return true; +} + + +void Animation::createSampleSweep(uint8_t seconds) { + clear(); + + const uint16_t sweepFrames = FRAMES_PER_SECOND * seconds; + const uint16_t totalFrames = sweepFrames * 2; // Up and down + + for (uint16_t frame = 0; frame < totalFrames; frame++) { + float progress; + if (frame < sweepFrames) { + // Sweep up: 0 → 1023 + progress = (float)frame / (sweepFrames - 1); + } else { + // Sweep down: 1023 → 0 + progress = 1.0f - ((float)(frame - sweepFrames) / (sweepFrames - 1)); + } + + uint16_t value = (uint16_t)(progress * 1023); + + for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { + setFrame(frame, ch, value); + } + } + + header.frameCount = totalFrames; + + // 🧩 Add keyframes to match the sweep motion + for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { + addKeyframe(ch, 0, 0); // Start at 0 + addKeyframe(ch, sweepFrames - 1, 1023); // Peak + addKeyframe(ch, totalFrames - 1, 0); // Return to 0 + } +} + +void Animation::createStaggeredSweep(uint8_t seconds) { + clear(); + + const uint16_t sweepFrames = FRAMES_PER_SECOND * seconds; + const uint16_t totalFrames = sweepFrames * 2; // Up and down + + for (uint16_t frame = 0; frame < totalFrames; frame++) { + float progress; + if (frame < sweepFrames) { + progress = (float)frame / (sweepFrames - 1); // 0 → 1 + } else { + progress = 1.0f - ((float)(frame - sweepFrames) / (sweepFrames - 1)); // 1 → 0 + } + + for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { + float channelProgress = (ch % 2 == 0) ? progress : (1.0f - progress); + uint16_t value = (uint16_t)(channelProgress * 1023); + setFrame(frame, ch, value); + } + } + + header.frameCount = totalFrames; +} diff --git a/animation.h b/animation.h new file mode 100644 index 0000000..28fbb16 --- /dev/null +++ b/animation.h @@ -0,0 +1,56 @@ +#ifndef ANIMATION_FILE_H +#define ANIMATION_FILE_H + +#include +#include "FS.h" +#include "FFat.h" +#include + +#define NUM_CHANNELS 5 +#define FRAMES_PER_SECOND 50 +#define MAX_DURATION_SECONDS 10 +#define MAX_FRAMES (FRAMES_PER_SECOND * MAX_DURATION_SECONDS) + +struct AnimationHeader { + char magic[4]; // 0–3 ANIM tag + uint16_t frameCount; // 4–5 Number of total frames + uint8_t version; // 6 + uint8_t frameRate; // 7 Frames per second + uint8_t reserved[8]; // 8–15 +}; + + +struct Keyframe { + uint8_t motorId; + uint16_t frame; + uint16_t position; +}; + + +class Animation { +public: + Animation(); + + void setFrame(uint16_t frameIndex, uint16_t channel, uint16_t value); + uint16_t getFrame(uint16_t frameIndex, uint16_t channel) const; + bool getFramePositions(uint16_t frameIndex, uint16_t* outPositions); + + void addKeyframe(uint8_t motorId, uint16_t frame, uint16_t position); + const std::vector& getKeyframes() const; + + void clear(); + uint16_t* getRawData(); // Optional: for bulk access + size_t getSize() const; + bool saveToFile(const char* filename); + bool loadFromFile(const char* filename); + uint16_t getFrameCount() const; + void createSampleSweep(uint8_t seconds); + void createStaggeredSweep(uint8_t seconds); + +private: + AnimationHeader header; + uint16_t data[MAX_FRAMES][NUM_CHANNELS]; + std::vector keyframes; +}; + +#endif diff --git a/feetech.cpp b/feetech.cpp index 51ac339..b9bdee4 100644 --- a/feetech.cpp +++ b/feetech.cpp @@ -344,7 +344,7 @@ void Feetech::write2Bytes(uint8_t id, byte instruction, uint16_t data) { void Feetech::pingAll(std::vector& successfulAddresses) { Serial.println("PINGING ALL 0-255"); successfulAddresses.clear(); // Clear any previous results - for (int i = 0; i < 254; i++) { + for (int i = 0; i < 20; i++) { //Serial.println(i); sendPing(i); uint8_t val = waitOnData1Byte(50); diff --git a/webpage.h b/webpage.h new file mode 100644 index 0000000..2611759 --- /dev/null +++ b/webpage.h @@ -0,0 +1,34 @@ +const char* webpage = R"rawliteral( + + + +ESP32 HTTP Console + + + +

ESP32 HTTP Console

+ + +
+ + +)rawliteral"; \ No newline at end of file