diff --git a/HansonServo.ino b/HansonServo.ino index e6efebb..d156ed5 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -1,3 +1,4 @@ + #include #include "feetech.h" #include "animation.h" @@ -14,6 +15,13 @@ #define CMD_DELETE_FILE 0x04 #define CMD_LOAD_FILE_CHUNK 0x05 +#define MAX_ANIMATION_SIZE (16 + MAX_FRAMES * NUM_CHANNELS * 2 + 512) // generous buffer +uint8_t animationBuffer[MAX_ANIMATION_SIZE]; +size_t receivedSize = 0; +size_t expectedSize = 0; + +Animation currentAnimation; + // ESP32 S2 PINOUT #define RX_PIN 17 // DI @@ -52,14 +60,14 @@ void setup() { return; } - sweep.clear(); - sweep.createSampleSweep(4); - sweep.saveToFile("/sweep.anim"); // sweep.clear(); - // sweep.createStaggeredSweep(4); - // sweep.saveToFile("/stagger.anim"); - //delay(9999); - sweep.printKeyframes(); + // sweep.createSampleSweep(4); + // sweep.saveToFile("/sweep.anim"); + // // sweep.clear(); + // // sweep.createStaggeredSweep(4); + // // sweep.saveToFile("/stagger.anim"); + // //delay(9999); + // sweep.printKeyframes(); // anim.clear(); @@ -137,29 +145,130 @@ void HandleSerialRequests() { payload += (char)Serial.read(); } - handleCommand(command, payload); + handleCommand(command, payload, length); } } } -void handleCommand(uint8_t command, const String& payload) { +void handleCommand(uint8_t command, const String& payload, uint8_t length) { 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\"}"); + + case CMD_LOAD_FILE_CHUNK: + handleAnimationChunk(payload, length); + break; + + // default: + // Serial.println("{\"error\":\"Unknown command\"}"); } } +void sendOkResponse(uint8_t command, const char* note = "chunk received") { + String payload = String("{\"status\":\"ok\",\"note\":\"") + note + "\"}"; + + uint16_t length = payload.length(); + uint8_t checksum = command ^ (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 handleAnimationChunk(const String& payload, uint8_t length) { + if (length < 4) { + sendOkResponse(CMD_LOAD_FILE_CHUNK, "fail: too short"); + return; + } + + const uint8_t* data = (const uint8_t*)payload.c_str(); + + uint16_t offset = (data[0] << 8) | data[1]; + uint16_t totalSize = (data[2] << 8) | data[3]; + const uint8_t* chunk = &data[4]; + size_t chunkSize = length - 4; + + if (offset == 0) { + receivedSize = 0; + expectedSize = totalSize; + } + + for (size_t i = 0; i < chunkSize; i++) { + if (offset + i < MAX_ANIMATION_SIZE) { + animationBuffer[offset + i] = chunk[i]; + } + } + + receivedSize += chunkSize; + + Serial.printf("Chunk received: offset=%d size=%d\n", offset, chunkSize); + Serial.printf("Total received: %d / %d\n", receivedSize, expectedSize); + + if (receivedSize >= expectedSize) { + Serial.println("Full animation received. Parsing…"); + loadAnimationFromBuffer(animationBuffer, expectedSize); + currentAnimation.saveToFile("savey.anim"); + sendOkResponse(CMD_LOAD_FILE_CHUNK, "final chunk"); + } else { + sendOkResponse(CMD_LOAD_FILE_CHUNK, "chunk received"); + } +} + + + + + +void loadAnimationFromBuffer(const uint8_t* buffer, size_t length) { + currentAnimation.clear(); + + memcpy(¤tAnimation.header, buffer, sizeof(AnimationHeader)); + + size_t offset = sizeof(AnimationHeader); + for (uint16_t frame = 0; frame < MAX_FRAMES; frame++) { + for (uint8_t channel = 0; channel < NUM_CHANNELS; channel++) { + uint16_t value = buffer[offset] | (buffer[offset + 1] << 8); + currentAnimation.setFrame(frame, channel, value); + offset += 2; + } + } + + uint16_t keyframeCount = buffer[offset] | (buffer[offset + 1] << 8); + offset += 2; + + for (uint16_t i = 0; i < keyframeCount; i++) { + uint8_t motorId = buffer[offset++]; + uint16_t frame = buffer[offset] | (buffer[offset + 1] << 8); + offset += 2; + uint16_t position = buffer[offset] | (buffer[offset + 1] << 8); + offset += 2; + currentAnimation.addKeyframe(motorId, frame, position); + } + + Serial.println("Animation loaded into memory."); + currentAnimation.printKeyframes(); +} + void sendReply(uint8_t command, const String& payload) { uint16_t length = payload.length(); // Supports up to 65,535 bytes @@ -317,10 +426,7 @@ void sendFileInChunks(const String& filename) { unsigned long duration = millis() - startTime; - String finalReply = "{\"status\":\"complete\",\"file\":\"" + filename + - "\",\"chunks\":" + chunksSent + - ",\"bytesSent\":" + totalSize + - ",\"durationMs\":" + duration + "}"; + String finalReply = "{\"status\":\"complete\",\"file\":\"" + filename + "\",\"chunks\":" + chunksSent + ",\"bytesSent\":" + totalSize + ",\"durationMs\":" + duration + "}"; sendReply(CMD_LOAD_FILE, finalReply); } diff --git a/animation.h b/animation.h index f9f9c06..cb00b2d 100644 --- a/animation.h +++ b/animation.h @@ -47,9 +47,9 @@ public: uint16_t getFrameCount() const; void createSampleSweep(uint8_t seconds); void createStaggeredSweep(uint8_t seconds); + AnimationHeader header; private: - AnimationHeader header; uint16_t data[MAX_FRAMES][NUM_CHANNELS]; std::vector keyframes;