diff --git a/HansonServo.ino b/HansonServo.ino index 95c6b14..ae3b799 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -8,13 +8,14 @@ #define HEADER1 0xAA #define HEADER2 0x55 -#define MAX_PAYLOAD_SIZE 10240 // 10 KB +#define MAX_PAYLOAD_SIZE 6000 // 10 KB uint8_t payload[MAX_PAYLOAD_SIZE]; // Global or static #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 // ESP32 S2 PINOUT @@ -23,7 +24,7 @@ uint8_t payload[MAX_PAYLOAD_SIZE]; // Global or static #define DE_PIN 33 // Driver Enable #define RE_PIN 3 // Receiver Enable -Animation animation; +Animation anim; Animation sweep; Animation stagger; @@ -55,12 +56,14 @@ void setup() { return; } + // ClearFiles(); + // // PrintFileList(); // sweep.clear(); // sweep.createSampleSweep(4); // sweep.saveToFile("/sweep.anim"); - // // sweep.clear(); - // // sweep.createStaggeredSweep(4); - // // sweep.saveToFile("/stagger.anim"); + // stagger.clear(); + // stagger.createStaggeredSweep(4); + // stagger.saveToFile("/stagger.anim"); // //delay(9999); // sweep.printKeyframes(); @@ -113,8 +116,14 @@ void HandleSerialRequests() { return; } - while (Serial.available() < length + 1) - ; // wait for payload + checksum + 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); @@ -152,6 +161,10 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) { handleDeleteFile(payload, length); break; + case CMD_SAVE_FILE: + handleSaveFile(payload, length); + break; + default: Serial.print("Unknown command: "); Serial.println(command, HEX); @@ -279,11 +292,97 @@ void handleLoadFile(const uint8_t* payload, uint16_t length) { void handleDeleteFile(const uint8_t* payload, uint16_t length) { } -void handleFileChunk(const uint8_t* payload, uint16_t length) { +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) { + if (length < 17) { + Serial.println("Payload too short for header"); + return false; + } + + // Parse header + memcpy(animation.header.magic, payload, 4); + if (strncmp(animation.header.magic, "ANIM", 4) != 0) { + Serial.println("Invalid magic header"); + return false; + } + + animation.header.frameCount = (payload[4] << 8) | payload[5]; + animation.header.version = payload[6]; + animation.header.frameRate = payload[7]; + memcpy(animation.header.reserved, payload + 8, 8); + + uint16_t keyframeCount = payload[16] | (payload[17] << 8); + + if (length < 17 + keyframeCount * sizeof(Keyframe)) { + Serial.println("Payload too short for keyframes"); + return false; + } + + // Parse keyframes + animation.clear(); + const uint8_t* ptr = payload + 18; + for (uint8_t i = 0; i < keyframeCount; i++) { + uint8_t motorId = ptr[0]; + uint16_t frame = ptr[1] | (ptr[2] << 8); + uint16_t position = ptr[3] | (ptr[4] << 8); + animation.addKeyframe(motorId, frame, position); + ptr += 5; + } + animation.saveToFile("/saved.anim"); + return true; +} + + + + + + unsigned long lastSend = 0; void loop() { @@ -334,6 +433,39 @@ void PrintFileList() { 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"); + } +} + void playAnimation(Animation& anim) { uint16_t positions[NUM_CHANNELS]; const uint16_t frameCount = anim.getFrameCount(); diff --git a/animation.cpp b/animation.cpp index b141033..99670a7 100644 --- a/animation.cpp +++ b/animation.cpp @@ -57,6 +57,7 @@ void Animation::printKeyframes() { void Animation::clear() { memset(data, 0, sizeof(data)); + keyframes.clear(); } uint16_t* Animation::getRawData() { @@ -91,15 +92,18 @@ bool Animation::saveToFile(const char* filename) { file.write((uint8_t*)&header, sizeof(header)); file.write((uint8_t*)data, sizeof(data)); - // Write keyframe count + // 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.write(kf.frame & 0xFF); // low byte + file.write((kf.frame >> 8) & 0xFF); // high byte + + file.write(kf.position & 0xFF); + file.write((kf.position >> 8) & 0xFF); } @@ -146,12 +150,20 @@ bool Animation::loadFromFile(const char* filename) { 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)) { + if (file.read(&kf.motorId, 1) != 1) { file.close(); return false; } + + uint8_t frameLow, frameHigh, posLow, posHigh; + if (file.read(&frameLow, 1) != 1 || file.read(&frameHigh, 1) != 1 || file.read(&posLow, 1) != 1 || file.read(&posHigh, 1) != 1) { + file.close(); + return false; + } + + kf.frame = (frameHigh << 8) | frameLow; + kf.position = (posHigh << 8) | posLow; + keyframes.push_back(kf); } @@ -188,11 +200,10 @@ void Animation::createSampleSweep(uint8_t seconds) { // 🧩 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 + 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) {