batch sending files, broken keyframe data

HansonServo.ino

@@ -1,10 +1,28 @@
+#include <base64.h>
 #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,11 +96,289 @@ void SetID(uint8_t oldID, uint8_t newID) {
   delay(1000);
 }
 
+unsigned long lastSend = 0;
 void loop() {
-  // put your main code here, to run repeatedly:
+  HandleSerialRequests();
-  PingAll();
 
+  // 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<uint8_t> successfulAddresses;

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<Keyframe>& 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;
+}

animation.h

@@ -0,0 +1,56 @@
+#ifndef ANIMATION_FILE_H
+#define ANIMATION_FILE_H
+
+#include <Arduino.h>
+#include "FS.h"
+#include "FFat.h"
+#include <vector>
+
+#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<Keyframe>& 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<Keyframe> keyframes;
+};
+
+#endif

feetech.cpp

@@ -344,7 +344,7 @@ void Feetech::write2Bytes(uint8_t id, byte instruction, uint16_t data) {
 void Feetech::pingAll(std::vector<uint8_t>& 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);

webpage.h

@@ -0,0 +1,34 @@
+const char* webpage = R"rawliteral(
+<!DOCTYPE html>
+<html>
+<head>
+<title>ESP32 HTTP Console</title>
+<script>
+function sendMessage() {
+  var msg = document.getElementById('message').value;
+  fetch('/send', {
+    method: 'POST',
+    headers: {
+      'Content-Type': 'application/x-www-form-urlencoded',
+    },
+    body: 'message=' + encodeURIComponent(msg)
+  })
+  .then(response => response.text())
+  .then(data => {
+    console.log(data);
+    document.getElementById('console').innerHTML += '<br>' + data;
+  })
+  .catch((error) => {
+    console.error('Error:', error);
+  });
+}
+</script>
+</head>
+<body>
+<h1>ESP32 HTTP Console</h1>
+<input id='message' type='text' placeholder='Type a message...'>
+<button onclick='sendMessage()'>Send</button>
+<div id='console' style='border:1px solid #000; height:200px; overflow:auto;'></div>
+</body>
+</html>
+)rawliteral";